From fe3dc924cec6fdb794a10d830bedeff18a0ea125 Mon Sep 17 00:00:00 2001 From: Capyblapy <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 8 Jul 2025 19:53:14 -0500 Subject: [PATCH 01/11] Started transfer of arcade drivetrain code wowiezowie - banyan --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 27 ++++ src/main/java/frc/robot/RobotContainer.java | 4 + .../robot/commands/ArcadeDriveCommand.java | 53 +++++++ .../frc/robot/subsystems/DriveSubsystem.java | 138 +++++++++++++++-- ....1.0.json => Phoenix6-frc2025-latest.json} | 144 ++++++++++++++---- .../{REVLib-2025.0.0.json => REVLib.json} | 15 +- 7 files changed, 332 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ArcadeDriveCommand.java rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-frc2025-latest.json} (75%) rename vendordeps/{REVLib-2025.0.0.json => REVLib.json} (86%) diff --git a/build.gradle b/build.gradle index 81c0f3b..1945af5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91aa6a3..d1eebd8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,6 +19,33 @@ public static class OperatorConstants { public static class DrivetrainConstants { public static final int kLeftMotorCANID = 10; + public static final int kOptionalLeftMotorCANID = 11; + public static final int kRightMotorCANID = 20; + public static final int kOptionalRightMotorCANID = 21; + + public static final double kTurnDivider = 2; + public static final double kSpeedDivider = 2.5; + + // Auto PID stuff + public static final double kV = 0; // Add x V output to overcome static friction + public static final double kS = 0; // A velocity target of 1 rps results in xV output + public static final double kP = 0.3; // An error of 1 rotation results in x V output + public static final double kI = 0.0; + public static final double kD = 0.1; // A velocity of 1 rps results in x V output + public static final double PeakVoltage = 10.0; + + public static final int maxVelocity = 30; // rps/s + public static final int maxAcceleration = 50; // rps + + // For Auto Potentially + public static boolean kLeftPositiveMovesForward = true; + public static boolean kRightPositiveMovesForward = true; + + // The distance travelled for a single rotation of the Kraken output shaft. + public static final double kDrivetrainGearRatio = (8.46/0.478536); + + // SmartDashboard update frequency for drive subsystem state in 20ms counts. + public static final int kTicksPerUpdate = 5; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..f00c680 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,6 +51,10 @@ private void configureBindings() { m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } + private void configureDefaultCommands() { + + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/ArcadeDriveCommand.java b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java new file mode 100644 index 0000000..11e8183 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java @@ -0,0 +1,53 @@ +// 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.DriveSubsystem; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ArcadeDriveCommand extends Command { + private final DriveSubsystem m_subsystem; + private XboxController m_driveController; + + /** + * Creates a new ArcadeDriveCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ArcadeDriveCommand(DriveSubsystem subsystem, XboxController driveController) { + m_subsystem = subsystem; + m_driveController = driveController; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // Note: We negate both axis values so that pushing the joystick forwards + // (which makes the readin more negative) increases the speed and twisting clockwise + // turns the robot clockwise. + m_subsystem.setArcadeSpeeds( + -m_driveController.getRightY(), + -m_driveController.getLeftX() + ); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 01d29cb..60b4a08 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -3,40 +3,150 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; +import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.DrivetrainConstants; +import frc.robot.Constants.*; +import frc.robot.commands.ArcadeDriveCommand; public class DriveSubsystem extends SubsystemBase { - private final TalonFX m_leftMotor = new TalonFX(DrivetrainConstants.kLeftMotorCANID); private final TalonFX m_rightMotor = new TalonFX(DrivetrainConstants.kRightMotorCANID); + private TalonFX m_optionalRightMotor; + + private final TalonFX m_leftMotor = new TalonFX(DrivetrainConstants.kLeftMotorCANID); + private TalonFX m_optionalLeftMotor; + private DifferentialDrive m_Drivetrain; + private double m_leftSpeed = 0.0; + private double m_rightSpeed = 0.0; + /** Creates a new DriveSubsystem. */ public DriveSubsystem() { // Right Motor - final MotorOutputConfigs m_rightMotorOutputConfigs = new MotorOutputConfigs(); - m_rightMotorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; // why is it a enum :sob: - m_rightMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake; + TalonFXConfiguration rightMotorConfig = new TalonFXConfiguration(); + rightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + rightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + m_rightMotor.getConfigurator().apply(rightMotorConfig); + SendableRegistry.setName(m_rightMotor, "DriveSubsystem", "rightMotor"); - final TalonFXConfigurator m_rightMotorConfigurator = m_rightMotor.getConfigurator(); - m_rightMotorConfigurator.apply(m_rightMotorOutputConfigs); + // Optional Right Motor + try { + m_optionalRightMotor = new TalonFX(DrivetrainConstants.kOptionalRightMotorCANID); + + // Setting up Config + TalonFXConfiguration optionalRightMotorConfig = new TalonFXConfiguration(); + optionalRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + optionalRightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + optionalRightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + // Saving + m_optionalRightMotor.getConfigurator().apply(optionalRightMotorConfig); + + // Setting as a follwer + m_optionalRightMotor.setControl( + new Follower(m_rightMotor.getDeviceID(), false) + ); + } + catch (Exception e) { + e.printStackTrace(); + } // Left Motor - final MotorOutputConfigs m_leftMotorOutputConfigs = new MotorOutputConfigs(); - m_leftMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake; + TalonFXConfiguration leftMotorConfig = new TalonFXConfiguration(); + leftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + leftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + leftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + m_leftMotor.getConfigurator().apply(leftMotorConfig); + SendableRegistry.setName(m_leftMotor, "DriveSubsystem", "leftMotor"); + + // Optional Left Motor + try { + m_optionalLeftMotor = new TalonFX(DrivetrainConstants.kOptionalLeftMotorCANID); - final TalonFXConfigurator m_leftMotorConfigurator = m_leftMotor.getConfigurator(); - m_leftMotorConfigurator.apply(m_leftMotorOutputConfigs); + // Setting up Config + TalonFXConfiguration optionalLeftMotorConfig = new TalonFXConfiguration(); + optionalLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + optionalLeftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + optionalLeftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + // Saving + m_optionalLeftMotor.getConfigurator().apply(optionalLeftMotorConfig); + + // Setting as follower + m_optionalLeftMotor.setControl( + new Follower(m_leftMotor.getDeviceID(), false) + ); + } + catch (Exception e) + { + // TODO: There are really two cases you want to catch. The first, when the follower + // motor controller doesn't exist, isn't an error. The second, where the motor exists + // but one of the later configuration calls fails, is an error. Generally, you would + // only dump a stack trace in error cases and you definitely don't want to do this in + // normal operation. I would suggest splitting this block into two try/except chunks, + // on that catches the missing controller and just outputs a status message to the log + // indicating that only one motor is in use, and the other catching the real errors + // and dumping the stack trace. + e.printStackTrace(); + } + + // Zeroing the encoders + m_leftMotor.setPosition(0); + m_rightMotor.setPosition(0); + + // Setting up the drive train + m_Drivetrain = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set); + SendableRegistry.setName(m_Drivetrain, "DriveSubsystem", "Drivetrain"); + } + + public void initDefaultCommand(XboxController Controller) + { + setDefaultCommand(new ArcadeDriveCommand(this, Controller)); + } + + public void setArcadeSpeeds(double joystickInput, double rotationInput) + { + m_leftSpeed = (joystickInput / DrivetrainConstants.kSpeedDivider); + m_rightSpeed = (rotationInput / DrivetrainConstants.kTurnDivider); + + // NOTE: We are making our own custom input modifications + //m_leftSpeed = Math.pow(m_leftSpeed, 3); + //m_rightSpeed = Math.pow(m_rightSpeed, 3); + + m_Drivetrain.arcadeDrive(m_leftSpeed, m_rightSpeed, true); + } - // TODO: Figure out how to make a differentialDrive w/ these motors + public double getSpeed(boolean bLeft) + { + if (bLeft) + { + return m_leftSpeed; + } + else + { + return m_rightSpeed; + } } @Override diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 75% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-frc2025-latest.json index 473f6a8..6f40c84 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "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": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "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": [ @@ -196,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +379,26 @@ ], "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.1.0", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +442,38 @@ "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" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib.json index cde6011..ac62be8 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", From b28177d74eae24e8bc4ceb24d133fc6522c872ec Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 2 Sep 2025 19:37:10 -0500 Subject: [PATCH 02/11] Installed navX and limelight deps --- src/main/java/frc/robot/LimelightHelpers.java | 1647 +++++++++++++++++ .../robot/subsystems/PositionSubsystem.java | 53 + vendordeps/Studica-2025.0.1.json | 71 + 3 files changed, 1771 insertions(+) create mode 100644 src/main/java/frc/robot/LimelightHelpers.java create mode 100644 src/main/java/frc/robot/subsystems/PositionSubsystem.java create mode 100644 vendordeps/Studica-2025.0.1.json diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java new file mode 100644 index 0000000..854968f --- /dev/null +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -0,0 +1,1647 @@ +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.LimelightHelpers.LimelightResults; +import frc.robot.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PositionSubsystem.java b/src/main/java/frc/robot/subsystems/PositionSubsystem.java new file mode 100644 index 0000000..46e6124 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PositionSubsystem.java @@ -0,0 +1,53 @@ +// 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.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; + +public class PositionSubsystem extends SubsystemBase { + private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); + + /** Creates a new PositionSubsystem. */ + public PositionSubsystem() {} + + /** + * Example command factory method. + * + * @return a command + */ + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); + } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 0000000..5010be0 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file From e6c549c7fbe59e5a3535c929f1f314b28007c58a Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 2 Sep 2025 19:46:58 -0500 Subject: [PATCH 03/11] Added very basic pose estimate code --- src/main/java/frc/robot/subsystems/PositionSubsystem.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/PositionSubsystem.java b/src/main/java/frc/robot/subsystems/PositionSubsystem.java index 46e6124..2d282eb 100644 --- a/src/main/java/frc/robot/subsystems/PositionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PositionSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; +import frc.robot.LimelightHelpers.PoseEstimate; public class PositionSubsystem extends SubsystemBase { private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); @@ -43,7 +44,11 @@ public boolean exampleCondition() { @Override public void periodic() { - // This method will be called once per scheduler run + // This method will be called once per scheduler + double robotYaw = m_gyro.getYaw(); + LimelightHelpers.SetRobotOrientation("", robotYaw, 0.0, 0.0, 0.0, 0.0, 0.0); + + PoseEstimate limelightEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); } @Override From 5157407a3ed21293cf44d1e78492cbb8c6ce5e41 Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 9 Sep 2025 19:43:12 -0500 Subject: [PATCH 04/11] Finished inital setup & started on limelightSubsystem --- src/main/java/frc/robot/RobotContainer.java | 82 +++++++++++++++---- .../robot/commands/ArcadeDriveCommand.java | 6 +- .../frc/robot/subsystems/DriveSubsystem.java | 14 +++- ...Subsystem.java => LimelightSubsystem.java} | 40 ++++----- 4 files changed, 97 insertions(+), 45 deletions(-) rename src/main/java/frc/robot/subsystems/{PositionSubsystem.java => LimelightSubsystem.java} (51%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f00c680..540d172 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,20 @@ 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.util.Optional; + +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.ExampleCommand; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.LimelightSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -20,18 +27,62 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + private final Optional m_driveSubsystem; + private final Optional m_limelightSubsystem; + private final Optional m_exampleSubsystem; // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + // Pose Estimators + // TODO: Figure out how to create the pose estimators + private DifferentialDrivePoseEstimator m_DrivePoseEstimator; + private DifferentialDrivePoseEstimator m_limeLightPoseEstimator; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // Init the subsystems + m_driveSubsystem = getSubsystem(DriveSubsystem.class, m_DrivePoseEstimator); + m_limelightSubsystem = getSubsystem(LimelightSubsystem.class, m_limeLightPoseEstimator); + m_exampleSubsystem = getSubsystem(ExampleSubsystem.class); + + // Configure the default commands + configureDefaultCommands(); + // Configure the trigger bindings configureBindings(); } + // Tom wrote this cool template to make the optional subsystem creation code in + // the constructor above a lot clearer. This is what clever coding looks like. + // Owen: I added the ability to pass object args through this for dependency injection, + // I hope this does not break anything or commited a coding sin or somthing + private static Optional getSubsystem(Class subsystemClass, Object... args) { + Optional iss; + try { + iss = Optional.ofNullable(subsystemClass.getDeclaredConstructor().newInstance(args)); + } catch (Exception e) { + iss = Optional.empty(); + // This is not tested! - Owen + DriverStation.reportWarning( + String.format( + "The %s was not found!", subsystemClass.getName()), + false + ); + } + return iss; + } + + private void configureDefaultCommands() { + if (m_driveSubsystem.isPresent()) + { + DriveSubsystem driveSubsystem = m_driveSubsystem.get(); + + driveSubsystem.initDefaultCommand(m_driverController); + } + } + /** * 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 @@ -42,17 +93,20 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + SmartDashboard.putBoolean("Example Subsystem", m_exampleSubsystem.isPresent()); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); - } + if (m_exampleSubsystem.isPresent()) + { + ExampleSubsystem exampleSubsystem = m_exampleSubsystem.get(); - private void configureDefaultCommands() { + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` + new Trigger(exampleSubsystem::exampleCondition) + .onTrue(new ExampleCommand(exampleSubsystem)); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // cancelling on release. + m_driverController.b().whileTrue(exampleSubsystem.exampleMethodCommand()); + } } /** @@ -62,6 +116,6 @@ private void configureDefaultCommands() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + return null; //Autos.exampleAuto(exampleSubsystem); } } diff --git a/src/main/java/frc/robot/commands/ArcadeDriveCommand.java b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java index 11e8183..aa59995 100644 --- a/src/main/java/frc/robot/commands/ArcadeDriveCommand.java +++ b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java @@ -4,20 +4,20 @@ package frc.robot.commands; import frc.robot.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** An example command that uses an example subsystem. */ public class ArcadeDriveCommand extends Command { private final DriveSubsystem m_subsystem; - private XboxController m_driveController; + private CommandXboxController m_driveController; /** * Creates a new ArcadeDriveCommand. * * @param subsystem The subsystem used by this command. */ - public ArcadeDriveCommand(DriveSubsystem subsystem, XboxController driveController) { + public ArcadeDriveCommand(DriveSubsystem subsystem, CommandXboxController driveController) { m_subsystem = subsystem; m_driveController = driveController; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 60b4a08..6f511cd 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -10,11 +10,14 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.*; import frc.robot.commands.ArcadeDriveCommand; @@ -30,8 +33,13 @@ public class DriveSubsystem extends SubsystemBase { private double m_leftSpeed = 0.0; private double m_rightSpeed = 0.0; + private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); + private final DifferentialDrivePoseEstimator m_poseEstimator; + /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { + public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator) { + m_poseEstimator = poseEstimator; + // Right Motor TalonFXConfiguration rightMotorConfig = new TalonFXConfiguration(); rightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -120,7 +128,7 @@ public DriveSubsystem() { SendableRegistry.setName(m_Drivetrain, "DriveSubsystem", "Drivetrain"); } - public void initDefaultCommand(XboxController Controller) + public void initDefaultCommand(CommandXboxController Controller) { setDefaultCommand(new ArcadeDriveCommand(this, Controller)); } diff --git a/src/main/java/frc/robot/subsystems/PositionSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java similarity index 51% rename from src/main/java/frc/robot/subsystems/PositionSubsystem.java rename to src/main/java/frc/robot/subsystems/LimelightSubsystem.java index 2d282eb..0008e82 100644 --- a/src/main/java/frc/robot/subsystems/PositionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -3,33 +3,17 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - -import com.studica.frc.AHRS; -import com.studica.frc.AHRS.NavXComType; - -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; -import frc.robot.LimelightHelpers.PoseEstimate; -public class PositionSubsystem extends SubsystemBase { - private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); +public class LimelightSubsystem extends SubsystemBase { + private final DifferentialDrivePoseEstimator m_poseEstimator; /** Creates a new PositionSubsystem. */ - public PositionSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); + public LimelightSubsystem(DifferentialDrivePoseEstimator poseEstimator) { + m_poseEstimator = poseEstimator; } /** @@ -45,10 +29,16 @@ public boolean exampleCondition() { @Override public void periodic() { // This method will be called once per scheduler - double robotYaw = m_gyro.getYaw(); - LimelightHelpers.SetRobotOrientation("", robotYaw, 0.0, 0.0, 0.0, 0.0, 0.0); - PoseEstimate limelightEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + // This is copy and pasted from limelight's documentation for testing. + LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); + if (limelightMeasurement.tagCount >= 2) { // Only trust measurement if we see multiple tags + m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); + m_poseEstimator.addVisionMeasurement( + limelightMeasurement.pose, + limelightMeasurement.timestampSeconds + ); + } } @Override From ef3ddea2ea5c35315d46afecd7649c59d6e2611f Mon Sep 17 00:00:00 2001 From: Recoil Robotics <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 16 Sep 2025 19:53:43 -0500 Subject: [PATCH 05/11] Refactored subsystem creation to now use factorys and dependency injection Only DriveSubsystem is updated --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 98 +++++++++------- .../robot/factorys/DriveSubsystemFactory.java | 30 +++++ .../frc/robot/factorys/TalonFXFactory.java | 20 ++++ .../frc/robot/subsystems/DriveSubsystem.java | 106 ++++++++---------- 5 files changed, 156 insertions(+), 101 deletions(-) create mode 100644 src/main/java/frc/robot/factorys/DriveSubsystemFactory.java create mode 100644 src/main/java/frc/robot/factorys/TalonFXFactory.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1eebd8..c4a8a56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,5 +47,8 @@ public static class DrivetrainConstants { // SmartDashboard update frequency for drive subsystem state in 20ms counts. public static final int kTicksPerUpdate = 5; + + // The track width in meters. + public static final double trackWidthMeters = 1; // TODO: Set Value! } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 540d172..a9288d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,18 +6,21 @@ import java.util.Optional; +import com.ctre.phoenix6.hardware.TalonFX; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; + import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; -import edu.wpi.first.math.estimator.PoseEstimator; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.DrivetrainConstants; import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.ExampleCommand; +import frc.robot.factorys.DriveSubsystemFactory; +import frc.robot.factorys.TalonFXFactory; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.ExampleSubsystem; -import frc.robot.subsystems.LimelightSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -28,24 +31,38 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Optional m_driveSubsystem; - private final Optional m_limelightSubsystem; - private final Optional m_exampleSubsystem; // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); + private final DifferentialDriveKinematics m_DriveKinematics = new DifferentialDriveKinematics(DrivetrainConstants.trackWidthMeters); + // Pose Estimators - // TODO: Figure out how to create the pose estimators - private DifferentialDrivePoseEstimator m_DrivePoseEstimator; - private DifferentialDrivePoseEstimator m_limeLightPoseEstimator; + private DifferentialDrivePoseEstimator m_DrivePoseEstimator = new DifferentialDrivePoseEstimator( + m_DriveKinematics, + m_gyro.getRotation2d(), + 0, + 0, + new Pose2d()); + + // Factorys + private TalonFXFactory m_TalonFXFactory = new TalonFXFactory(); + private DriveSubsystemFactory m_DriveSubsystemFactory = new DriveSubsystemFactory(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // Init DriveSubsystem + Optional rightLead = m_TalonFXFactory.construct(DrivetrainConstants.kRightMotorCANID); + Optional leftLead = m_TalonFXFactory.construct(DrivetrainConstants.kLeftMotorCANID); + Optional rightFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalRightMotorCANID); + Optional leftFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalLeftMotorCANID); + m_driveSubsystem = m_DriveSubsystemFactory.construct(m_DrivePoseEstimator, m_gyro, rightLead, leftLead, rightFollower, leftFollower); + // Init the subsystems - m_driveSubsystem = getSubsystem(DriveSubsystem.class, m_DrivePoseEstimator); - m_limelightSubsystem = getSubsystem(LimelightSubsystem.class, m_limeLightPoseEstimator); - m_exampleSubsystem = getSubsystem(ExampleSubsystem.class); + //m_limelightSubsystem = getSubsystem(LimelightSubsystem.class, m_limeLightPoseEstimator); + //m_exampleSubsystem = getSubsystem(ExampleSubsystem.class); // Configure the default commands configureDefaultCommands(); @@ -58,21 +75,22 @@ public RobotContainer() { // the constructor above a lot clearer. This is what clever coding looks like. // Owen: I added the ability to pass object args through this for dependency injection, // I hope this does not break anything or commited a coding sin or somthing - private static Optional getSubsystem(Class subsystemClass, Object... args) { - Optional iss; - try { - iss = Optional.ofNullable(subsystemClass.getDeclaredConstructor().newInstance(args)); - } catch (Exception e) { - iss = Optional.empty(); - // This is not tested! - Owen - DriverStation.reportWarning( - String.format( - "The %s was not found!", subsystemClass.getName()), - false - ); - } - return iss; - } + // private static Optional getSubsystem(Class subsystemClass, Object... args) { + // Optional iss; + // try { + + // iss = Optional.ofNullable(subsystemClass.getDeclaredConstructor().newInstance(args)); + // } catch (Exception e) { + // iss = Optional.empty(); + // // This is not tested! - Owen + // DriverStation.reportWarning( + // String.format( + // "The %s was not found!", subsystemClass.getName()), + // false + // ); + // } + // return iss; + // } private void configureDefaultCommands() { if (m_driveSubsystem.isPresent()) @@ -93,20 +111,20 @@ private void configureDefaultCommands() { * joysticks}. */ private void configureBindings() { - SmartDashboard.putBoolean("Example Subsystem", m_exampleSubsystem.isPresent()); + // SmartDashboard.putBoolean("Example Subsystem", m_exampleSubsystem.isPresent()); - if (m_exampleSubsystem.isPresent()) - { - ExampleSubsystem exampleSubsystem = m_exampleSubsystem.get(); + // if (m_exampleSubsystem.isPresent()) + // { + // ExampleSubsystem exampleSubsystem = m_exampleSubsystem.get(); - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(exampleSubsystem)); + // // Schedule `ExampleCommand` when `exampleCondition` changes to `true` + // new Trigger(exampleSubsystem::exampleCondition) + // .onTrue(new ExampleCommand(exampleSubsystem)); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(exampleSubsystem.exampleMethodCommand()); - } + // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // // cancelling on release. + // m_driverController.b().whileTrue(exampleSubsystem.exampleMethodCommand()); + // } } /** diff --git a/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java new file mode 100644 index 0000000..2ac15bc --- /dev/null +++ b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java @@ -0,0 +1,30 @@ +package frc.robot.factorys; + +import java.util.Optional; +import com.ctre.phoenix6.hardware.TalonFX; +import com.studica.frc.AHRS; + +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import frc.robot.subsystems.DriveSubsystem; + +public class DriveSubsystemFactory { + public DriveSubsystemFactory() { + } + + public Optional construct(DifferentialDrivePoseEstimator poseEstimator, AHRS gyro, + Optional rightLead, Optional leftLead, Optional rightFollower, + Optional leftFollower) { + + if (rightLead.isEmpty() || leftLead.isEmpty()) { + return Optional.empty(); + } + + DriveSubsystem driveSubsystem = new DriveSubsystem(poseEstimator, gyro, rightLead.get(), leftLead.get()); + + if (rightFollower.isPresent() && leftFollower.isPresent()) { + driveSubsystem.setFollowers(rightFollower.get(), leftFollower.get()); + } + + return Optional.of(driveSubsystem); + } +} diff --git a/src/main/java/frc/robot/factorys/TalonFXFactory.java b/src/main/java/frc/robot/factorys/TalonFXFactory.java new file mode 100644 index 0000000..1a84899 --- /dev/null +++ b/src/main/java/frc/robot/factorys/TalonFXFactory.java @@ -0,0 +1,20 @@ +package frc.robot.factorys; + +import java.util.Optional; + +import com.ctre.phoenix6.hardware.TalonFX; + +public class TalonFXFactory { + public TalonFXFactory() {} + + public Optional construct(int CANID) { + Optional newMotor; + try { + newMotor = Optional.of(new TalonFX(CANID)); + } catch (Exception e) { + newMotor = Optional.empty(); + } + + return newMotor; + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 6f511cd..46e1b0c 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -22,10 +22,10 @@ import frc.robot.commands.ArcadeDriveCommand; public class DriveSubsystem extends SubsystemBase { - private final TalonFX m_rightMotor = new TalonFX(DrivetrainConstants.kRightMotorCANID); + private TalonFX m_rightMotor; private TalonFX m_optionalRightMotor; - private final TalonFX m_leftMotor = new TalonFX(DrivetrainConstants.kLeftMotorCANID); + private TalonFX m_leftMotor; private TalonFX m_optionalLeftMotor; private DifferentialDrive m_Drivetrain; @@ -33,12 +33,15 @@ public class DriveSubsystem extends SubsystemBase { private double m_leftSpeed = 0.0; private double m_rightSpeed = 0.0; - private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); private final DifferentialDrivePoseEstimator m_poseEstimator; + private final AHRS m_gyro; /** Creates a new DriveSubsystem. */ - public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator) { + public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator, AHRS gyro, TalonFX rightMotor, TalonFX leftMotor) { m_poseEstimator = poseEstimator; + m_gyro = gyro; + m_rightMotor = rightMotor; + m_leftMotor = leftMotor; // Right Motor TalonFXConfiguration rightMotorConfig = new TalonFXConfiguration(); @@ -51,30 +54,6 @@ public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator) { m_rightMotor.getConfigurator().apply(rightMotorConfig); SendableRegistry.setName(m_rightMotor, "DriveSubsystem", "rightMotor"); - // Optional Right Motor - try { - m_optionalRightMotor = new TalonFX(DrivetrainConstants.kOptionalRightMotorCANID); - - // Setting up Config - TalonFXConfiguration optionalRightMotorConfig = new TalonFXConfiguration(); - optionalRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - optionalRightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - optionalRightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) - .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); - - // Saving - m_optionalRightMotor.getConfigurator().apply(optionalRightMotorConfig); - - // Setting as a follwer - m_optionalRightMotor.setControl( - new Follower(m_rightMotor.getDeviceID(), false) - ); - } - catch (Exception e) { - e.printStackTrace(); - } - // Left Motor TalonFXConfiguration leftMotorConfig = new TalonFXConfiguration(); leftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -86,39 +65,6 @@ public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator) { m_leftMotor.getConfigurator().apply(leftMotorConfig); SendableRegistry.setName(m_leftMotor, "DriveSubsystem", "leftMotor"); - // Optional Left Motor - try { - m_optionalLeftMotor = new TalonFX(DrivetrainConstants.kOptionalLeftMotorCANID); - - // Setting up Config - TalonFXConfiguration optionalLeftMotorConfig = new TalonFXConfiguration(); - optionalLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - optionalLeftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - optionalLeftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) - .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); - - // Saving - m_optionalLeftMotor.getConfigurator().apply(optionalLeftMotorConfig); - - // Setting as follower - m_optionalLeftMotor.setControl( - new Follower(m_leftMotor.getDeviceID(), false) - ); - } - catch (Exception e) - { - // TODO: There are really two cases you want to catch. The first, when the follower - // motor controller doesn't exist, isn't an error. The second, where the motor exists - // but one of the later configuration calls fails, is an error. Generally, you would - // only dump a stack trace in error cases and you definitely don't want to do this in - // normal operation. I would suggest splitting this block into two try/except chunks, - // on that catches the missing controller and just outputs a status message to the log - // indicating that only one motor is in use, and the other catching the real errors - // and dumping the stack trace. - e.printStackTrace(); - } - // Zeroing the encoders m_leftMotor.setPosition(0); m_rightMotor.setPosition(0); @@ -128,6 +74,44 @@ public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator) { SendableRegistry.setName(m_Drivetrain, "DriveSubsystem", "Drivetrain"); } + public void setFollowers(TalonFX optionalRight, TalonFX optionalLeft) { + m_optionalRightMotor = optionalRight; + + // Setting up Config + TalonFXConfiguration optionalRightMotorConfig = new TalonFXConfiguration(); + optionalRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + optionalRightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + optionalRightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + // Saving + m_optionalRightMotor.getConfigurator().apply(optionalRightMotorConfig); + + // Setting as a follwer + m_optionalRightMotor.setControl( + new Follower(m_rightMotor.getDeviceID(), false) + ); + + m_optionalLeftMotor = optionalLeft; + + // Setting up Config + TalonFXConfiguration optionalLeftMotorConfig = new TalonFXConfiguration(); + optionalLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + optionalLeftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + optionalLeftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage)) + .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage)); + + // Saving + m_optionalLeftMotor.getConfigurator().apply(optionalLeftMotorConfig); + + // Setting as follower + m_optionalLeftMotor.setControl( + new Follower(m_leftMotor.getDeviceID(), false) + ); + } + public void initDefaultCommand(CommandXboxController Controller) { setDefaultCommand(new ArcadeDriveCommand(this, Controller)); From 62cd09133fc255b3459908acc045dc637c925250 Mon Sep 17 00:00:00 2001 From: 15 - Phosphorus <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 23 Sep 2025 19:35:04 -0500 Subject: [PATCH 06/11] Robot should now track its distance and rotation (hopefully) Distance is in meters now after some quick unit conversion. The robot will keep track of how far it goes and it's rotation with a gyrometer. --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/DriveSubsystem.java | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c4a8a56..99beebf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,6 +44,7 @@ public static class DrivetrainConstants { // The distance travelled for a single rotation of the Kraken output shaft. public static final double kDrivetrainGearRatio = (8.46/0.478536); + public static final double kWheelRadiusMeters = (4.0 / 2.0) * 0.0254; // Four Inch Wheels // SmartDashboard update frequency for drive subsystem state in 20ms counts. public static final int kTicksPerUpdate = 5; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 46e1b0c..1601ae9 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -144,6 +144,9 @@ public double getSpeed(boolean bLeft) @Override public void periodic() { // This method will be called once per scheduler run + + m_poseEstimator.update( + m_gyro.getRotation2d(), m_leftMotor.getPosition(), m_rightMotor.getPosition()); } @Override From 176fe6cfe4c7e6d06db20fe77f9da37b4d9035d2 Mon Sep 17 00:00:00 2001 From: 15 - Phosphorus <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 23 Sep 2025 19:52:47 -0500 Subject: [PATCH 07/11] Created getPose2d that cn be called to receive information on the robot's position --- .../java/frc/robot/subsystems/DriveSubsystem.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 1601ae9..077419e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -14,6 +14,7 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -141,12 +142,23 @@ public double getSpeed(boolean bLeft) } } + + // Wrapping robot position inside of getposition + public Pose2d getPose2d(){ + return m_poseEstimator.getEstimatedPosition(); + } + @Override public void periodic() { // This method will be called once per scheduler run + double leftRotations = m_leftMotor.getPosition().getValueAsDouble(); + double leftDistanceMeters = leftRotations * 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters / DrivetrainConstants.kDrivetrainGearRatio; + + double rightRotations = m_rightMotor.getPosition().getValueAsDouble(); + double rightDistanceMeters = rightRotations * 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters / DrivetrainConstants.kDrivetrainGearRatio; m_poseEstimator.update( - m_gyro.getRotation2d(), m_leftMotor.getPosition(), m_rightMotor.getPosition()); + m_gyro.getRotation2d(), leftDistanceMeters, rightDistanceMeters); } @Override From b78c8931f0e3477290d0d0b861041600b9a09090 Mon Sep 17 00:00:00 2001 From: 23 - Vanadium <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 30 Sep 2025 19:52:36 -0500 Subject: [PATCH 08/11] Made GetRobotRelativeSpeeds --- src/main/java/frc/robot/Constants.java | 3 ++ .../frc/robot/subsystems/DriveSubsystem.java | 44 ++++++++++++++++--- vendordeps/PathplannerLib-2025.2.7.json | 38 ++++++++++++++++ 3 files changed, 78 insertions(+), 7 deletions(-) create mode 100644 vendordeps/PathplannerLib-2025.2.7.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99beebf..fae1438 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,6 +45,9 @@ public static class DrivetrainConstants { // The distance travelled for a single rotation of the Kraken output shaft. public static final double kDrivetrainGearRatio = (8.46/0.478536); public static final double kWheelRadiusMeters = (4.0 / 2.0) * 0.0254; // Four Inch Wheels + public static final double kWheelCircumfrance = 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters; + + public static final double kWheelDistanceFromCenterOfRotation = 0.5; // meters, need to mesure. // SmartDashboard update frequency for drive subsystem state in 20ms counts. public static final int kTicksPerUpdate = 5; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 077419e..ebb3776 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -142,23 +143,52 @@ public double getSpeed(boolean bLeft) } } - // Wrapping robot position inside of getposition public Pose2d getPose2d(){ return m_poseEstimator.getEstimatedPosition(); } + // Resets the drivetrains pose estimator to zero. + public void resetPose(){ + m_leftMotor.setPosition(0); + m_rightMotor.setPosition(0); + m_poseEstimator.resetPose(Pose2d.kZero); + } + + // Returns a robot relative ChassisSpeeds object based on the avrg linear velocity + // in meters per second and avrg anglear velocity in readians per second + public ChassisSpeeds getRobotRelativeSpeeds(){ + // Linear Velocity in meters per second + double leftMPS = m_leftMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kWheelCircumfrance; + double rightMPS = m_rightMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kWheelCircumfrance; + + // Anglear Velocity in radians per second + double leftRPS = leftMPS/DrivetrainConstants.kWheelDistanceFromCenterOfRotation; + double rightRPS = rightMPS/DrivetrainConstants.kWheelDistanceFromCenterOfRotation; + + // Avrging them togther + double linearVelocity = (leftMPS+rightMPS)/2; + double AnglearVelocity = (leftRPS+rightRPS)/2; + + // Making Chassis Speeds + return new ChassisSpeeds(linearVelocity, 0, AnglearVelocity); + } + + // Gives the drivetrain a new drive command based on a robot relative + // chassis speed object. + public void driveRobotRelative(ChassisSpeeds relativeChassisSpeed){ + m_Drivetrain.arcadeDrive(relativeChassisSpeed.vxMetersPerSecond, + relativeChassisSpeed.omegaRadiansPerSecond); + } + @Override public void periodic() { // This method will be called once per scheduler run - double leftRotations = m_leftMotor.getPosition().getValueAsDouble(); - double leftDistanceMeters = leftRotations * 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters / DrivetrainConstants.kDrivetrainGearRatio; - - double rightRotations = m_rightMotor.getPosition().getValueAsDouble(); - double rightDistanceMeters = rightRotations * 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters / DrivetrainConstants.kDrivetrainGearRatio; m_poseEstimator.update( - m_gyro.getRotation2d(), leftDistanceMeters, rightDistanceMeters); + m_gyro.getRotation2d(), + m_leftMotor.getPosition().getValueAsDouble(), + m_rightMotor.getPosition().getValueAsDouble()); } @Override diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json new file mode 100644 index 0000000..d3f84e5 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.7.json", + "name": "PathplannerLib", + "version": "2025.2.7", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.7" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.7", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From cebd6edb9dc834220774e08d5c8ea100632db57f Mon Sep 17 00:00:00 2001 From: 23 - Vanadium <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 7 Oct 2025 19:59:25 -0500 Subject: [PATCH 09/11] Finished pathplanner required functions & started on loading autos --- build.gradle | 2 +- .../deploy/pathplanner/autos/CoolAuto.auto | 19 +++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 34 ++++++++++++ src/main/java/frc/robot/Constants.java | 2 - src/main/java/frc/robot/RobotContainer.java | 10 +++- .../robot/factorys/DriveSubsystemFactory.java | 9 ++-- .../frc/robot/subsystems/DriveSubsystem.java | 24 ++++----- .../java/frc/robot/utils/getAutoNames.java | 18 +++++++ 10 files changed, 153 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/CoolAuto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc/robot/utils/getAutoNames.java diff --git a/build.gradle b/build.gradle index 1945af5..c5ce1a7 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/autos/CoolAuto.auto b/src/main/deploy/pathplanner/autos/CoolAuto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CoolAuto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..afb9553 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.1123076923076916, + "y": 7.0 + }, + "prevControl": { + "x": 5.1123076923076916, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..c64d29d --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,34 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "New Folder" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fae1438..37d9337 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,8 +47,6 @@ public static class DrivetrainConstants { public static final double kWheelRadiusMeters = (4.0 / 2.0) * 0.0254; // Four Inch Wheels public static final double kWheelCircumfrance = 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters; - public static final double kWheelDistanceFromCenterOfRotation = 0.5; // meters, need to mesure. - // SmartDashboard update frequency for drive subsystem state in 20ms counts. public static final int kTicksPerUpdate = 5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9288d9..496e1ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,11 @@ package frc.robot; +import java.io.File; import java.util.Optional; import com.ctre.phoenix6.hardware.TalonFX; +import com.pathplanner.lib.auto.AutoBuilder; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; @@ -58,12 +60,18 @@ public RobotContainer() { Optional leftLead = m_TalonFXFactory.construct(DrivetrainConstants.kLeftMotorCANID); Optional rightFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalRightMotorCANID); Optional leftFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalLeftMotorCANID); - m_driveSubsystem = m_DriveSubsystemFactory.construct(m_DrivePoseEstimator, m_gyro, rightLead, leftLead, rightFollower, leftFollower); + m_driveSubsystem = m_DriveSubsystemFactory.construct(m_DrivePoseEstimator, m_DriveKinematics, m_gyro, rightLead, leftLead, rightFollower, leftFollower); // Init the subsystems //m_limelightSubsystem = getSubsystem(LimelightSubsystem.class, m_limeLightPoseEstimator); //m_exampleSubsystem = getSubsystem(ExampleSubsystem.class); + // Init Autos (/home/lvuser/deploy/pathplanner/autos) + File directory = new File("/home/lvuser/deploy/pathplanner/autos"); + + // Init Chooser + // autoChooser = AutoBuilder.buildAutoChooser(); + // Configure the default commands configureDefaultCommands(); diff --git a/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java index 2ac15bc..afc5b84 100644 --- a/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java +++ b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java @@ -5,21 +5,24 @@ import com.studica.frc.AHRS; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import frc.robot.subsystems.DriveSubsystem; public class DriveSubsystemFactory { public DriveSubsystemFactory() { } - public Optional construct(DifferentialDrivePoseEstimator poseEstimator, AHRS gyro, - Optional rightLead, Optional leftLead, Optional rightFollower, + public Optional construct(DifferentialDrivePoseEstimator poseEstimator, + DifferentialDriveKinematics kinematics, + AHRS gyro, Optional rightLead, Optional leftLead, Optional rightFollower, Optional leftFollower) { if (rightLead.isEmpty() || leftLead.isEmpty()) { return Optional.empty(); } - DriveSubsystem driveSubsystem = new DriveSubsystem(poseEstimator, gyro, rightLead.get(), leftLead.get()); + DriveSubsystem driveSubsystem = new DriveSubsystem(poseEstimator, kinematics, gyro, rightLead.get(), + leftLead.get()); if (rightFollower.isPresent() && leftFollower.isPresent()) { driveSubsystem.setFollowers(rightFollower.get(), leftFollower.get()); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index ebb3776..fe79d2b 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -16,6 +16,8 @@ import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -35,12 +37,14 @@ public class DriveSubsystem extends SubsystemBase { private double m_leftSpeed = 0.0; private double m_rightSpeed = 0.0; + private final DifferentialDriveKinematics m_kinematics; private final DifferentialDrivePoseEstimator m_poseEstimator; private final AHRS m_gyro; /** Creates a new DriveSubsystem. */ - public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator, AHRS gyro, TalonFX rightMotor, TalonFX leftMotor) { + public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator, DifferentialDriveKinematics kinematics, AHRS gyro, TalonFX rightMotor, TalonFX leftMotor) { m_poseEstimator = poseEstimator; + m_kinematics = kinematics; m_gyro = gyro; m_rightMotor = rightMotor; m_leftMotor = leftMotor; @@ -157,21 +161,15 @@ public void resetPose(){ // Returns a robot relative ChassisSpeeds object based on the avrg linear velocity // in meters per second and avrg anglear velocity in readians per second + // Currently we are asuming that their is no scale for the motors, we cannot find anywhere to set the scale. public ChassisSpeeds getRobotRelativeSpeeds(){ // Linear Velocity in meters per second - double leftMPS = m_leftMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kWheelCircumfrance; - double rightMPS = m_rightMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kWheelCircumfrance; + double leftMPS = m_leftMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kDrivetrainGearRatio * DrivetrainConstants.kWheelCircumfrance; + double rightMPS = m_rightMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kDrivetrainGearRatio * DrivetrainConstants.kWheelCircumfrance; - // Anglear Velocity in radians per second - double leftRPS = leftMPS/DrivetrainConstants.kWheelDistanceFromCenterOfRotation; - double rightRPS = rightMPS/DrivetrainConstants.kWheelDistanceFromCenterOfRotation; - - // Avrging them togther - double linearVelocity = (leftMPS+rightMPS)/2; - double AnglearVelocity = (leftRPS+rightRPS)/2; - - // Making Chassis Speeds - return new ChassisSpeeds(linearVelocity, 0, AnglearVelocity); + // Make wheelSpeeds object from MPS & converts it to chasis speeds + DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftMPS, rightMPS); + return m_kinematics.toChassisSpeeds(wheelSpeeds); } // Gives the drivetrain a new drive command based on a robot relative diff --git a/src/main/java/frc/robot/utils/getAutoNames.java b/src/main/java/frc/robot/utils/getAutoNames.java new file mode 100644 index 0000000..a4092be --- /dev/null +++ b/src/main/java/frc/robot/utils/getAutoNames.java @@ -0,0 +1,18 @@ +package frc.robot.utils; + +import java.io.File; +import java.util.ArrayList; +import java.util.List; + +// This goes to "/home/lvuser/deploy/pathplanner/autos" and grabs the names of all .auto files +// We are asuming that the PathPlannerAuto(string) is the file name of the auto +// We are asuming that the files are always in the "/home/lvuser/deploy/pathplanner/autos" +// We found this information at [URL HERE] +public class getAutoNames { + public List main() { + File directory = new File("/home/lvuser/deploy/pathplanner/autos"); + + List fileNames = new ArrayList<>(); + return fileNames; + } +} From ad2c5ab11e42c159a01e40f411938357ec1962a2 Mon Sep 17 00:00:00 2001 From: Capyblapy <111537871+Capyblapy@users.noreply.github.com> Date: Wed, 8 Oct 2025 11:08:02 -0500 Subject: [PATCH 10/11] Finished the get auto names util --- .../java/frc/robot/utils/getAutoNames.java | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/utils/getAutoNames.java b/src/main/java/frc/robot/utils/getAutoNames.java index a4092be..e864220 100644 --- a/src/main/java/frc/robot/utils/getAutoNames.java +++ b/src/main/java/frc/robot/utils/getAutoNames.java @@ -2,17 +2,41 @@ import java.io.File; import java.util.ArrayList; -import java.util.List; // This goes to "/home/lvuser/deploy/pathplanner/autos" and grabs the names of all .auto files // We are asuming that the PathPlannerAuto(string) is the file name of the auto // We are asuming that the files are always in the "/home/lvuser/deploy/pathplanner/autos" -// We found this information at [URL HERE] +// We found this information at https://github.com/mjansen4857/pathplanner/wiki/PathPlannerLib:-Java-Usage public class getAutoNames { - public List main() { - File directory = new File("/home/lvuser/deploy/pathplanner/autos"); + public String[] main() { + // Creating an ArrayList so I can fill it with auto file names w/o the extention. + ArrayList autoNames = new ArrayList(); - List fileNames = new ArrayList<>(); - return fileNames; + // Getting the folder and files. + File folder = new File("/home/lvuser/deploy/pathplanner/autos"); + File[] listOfFiles = folder.listFiles(); + + // Checking if the files exist + if (listOfFiles != null) { + for (File file : listOfFiles) { + if (file.isFile()) { + String fullName = file.getName(); // Returns [file_name].[extention] + String[] splitName = fullName.split("."); // Splitting the file name into seprate parts. + + // If its an auto file then add it to the list. + if (splitName[1] == "auto") { + autoNames.add(splitName[0]); + System.out.println(splitName[0]); + } + } + } + } + + // Converting the ArrayList into a normal array for memory optimization & saftey + String[] autoNamesArray = new String[autoNames.size()]; + autoNames.toArray(autoNamesArray); + + // Returning the new array + return autoNamesArray; } } From 144527347e7234acf5bf3691bbd9cea240a6dd70 Mon Sep 17 00:00:00 2001 From: 23 - Vanadium <111537871+Capyblapy@users.noreply.github.com> Date: Tue, 14 Oct 2025 18:32:47 -0500 Subject: [PATCH 11/11] Commented out auto code --- simgui-ds.json | 92 +++++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 93 insertions(+), 1 deletion(-) create mode 100644 simgui-ds.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "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, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 496e1ec..4679afa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,7 @@ public RobotContainer() { //m_exampleSubsystem = getSubsystem(ExampleSubsystem.class); // Init Autos (/home/lvuser/deploy/pathplanner/autos) - File directory = new File("/home/lvuser/deploy/pathplanner/autos"); + //File directory = new File("/home/lvuser/deploy/pathplanner/autos"); // Init Chooser // autoChooser = AutoBuilder.buildAutoChooser();