diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9a9ca7b --- /dev/null +++ b/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5e6ede8 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,61 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.dependency.enableDependencyCheckup": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..d459245 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 2890 +} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index c63292c..134897c 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -3,7 +3,11 @@ "front": -10.9, "left": 10.9 }, +<<<<<<< HEAD "absoluteEncoderOffset": 216.474609375, +======= + "absoluteEncoderOffset": 216.38671875, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "drive": { "type": "sparkflex_neo", "id": 12, diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index 3730a3c..b96e756 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -3,7 +3,11 @@ "front": -10.9, "left": -10.9 }, +<<<<<<< HEAD "absoluteEncoderOffset": 191.689453125, +======= + "absoluteEncoderOffset": 191.6015625, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "drive": { "type": "sparkflex_neo", "id": 11, diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index c127faa..087af99 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -3,7 +3,11 @@ "front": 10.9, "left": 10.9 }, +<<<<<<< HEAD "absoluteEncoderOffset": 34.1015625, +======= + "absoluteEncoderOffset": 198.896484375, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "drive": { "type": "sparkflex_neo", "id": 13, diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 923e281..5646506 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -3,7 +3,11 @@ "front": 10.9, "left": -10.9 }, +<<<<<<< HEAD "absoluteEncoderOffset": 199.775390625, +======= + "absoluteEncoderOffset": 231.064453125, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "drive": { "type": "sparkflex_neo", "id": 10, diff --git a/src/main/deploy/swerve/neo/modules/physicalproperties.json b/src/main/deploy/swerve/neo/modules/physicalproperties.json index 65fa27e..2d0abf2 100644 --- a/src/main/deploy/swerve/neo/modules/physicalproperties.json +++ b/src/main/deploy/swerve/neo/modules/physicalproperties.json @@ -1,6 +1,10 @@ { "optimalVoltage": 12, +<<<<<<< HEAD "robotMass": 93.5, +======= + "robotMass": 51.75, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "wheelGripCoefficientOfFriction": 1.85, "currentLimit": { "drive": 40, diff --git a/src/main/deploy/swerve/neo/modules/pidfproperties.json b/src/main/deploy/swerve/neo/modules/pidfproperties.json index fc10acd..2c1b49d 100644 --- a/src/main/deploy/swerve/neo/modules/pidfproperties.json +++ b/src/main/deploy/swerve/neo/modules/pidfproperties.json @@ -7,9 +7,15 @@ "iz": 0 }, "angle": { +<<<<<<< HEAD "p": 0.005, "i": 0, "d": 0, +======= + "p": 0.002, + "i": 0.0, + "d": 0.0, +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "f": 0, "iz": 0 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 92bbc9e..eb464b6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,7 +33,11 @@ public final class Constants { private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming"); +<<<<<<< HEAD public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound +======= + public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag public static final double MAX_SPEED = Units.feetToMeters(14.5); @@ -63,6 +67,7 @@ public final class Constants { } public static class ShooterConstants { +<<<<<<< HEAD private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); @@ -70,15 +75,44 @@ public final class Constants { public static void getShooterVelocity() { SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5); +======= + private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter RPM", -1000) + .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + + public static double SHOOTER_RPM = -0.6; + public static double SHOOTER_POWER = -0.45; + + public static void getShooterVelocity() { + SHOOTER_RPM = shooterVelocity.getDouble(-1000); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } public static final int CENTER_SHOOTER_MOTOR_ID = 42; public static final int LEFT_SHOOTER_MOTOR_ID = 41; public static final int RIGHT_SHOOTER_MOTOR_ID = 40; public static final int INDEXER_MOTOR_ID = 43; +<<<<<<< HEAD public static final double INDEXER_MOTOR_SPEED = 0.6; public static final int LEFT_ACTUATOR_ID = 44; public static final int RIGHT_ACTUATOR_ID = 45; +======= + public static double INDEXER_AND_RAMP_MOTOR_RPM; + + public static double SHOOTER_MOTOR_P = 0.001; + public static double SHOOTER_MOTOR_I = 0; + public static double SHOOTER_MOTOR_D = 0; + private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer Speed", 1000) + .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + + // this method called in robot periodic so values updated in elastic are + // constantly read and applied to RAMP_MOTOR_SPEED + public static void getIndexerAndRampMotorRPM() { + INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(1000); + } + + public static final int LEFT_ACTUATOR_PWM_PORT = 1; + public static final int RIGHT_ACTUATOR_PWM_PORT = 3; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f public static double DESIRED_POTENTIOMETER_DISTANCE; @@ -86,6 +120,7 @@ public final class Constants { } public static class IntakeConstants { +<<<<<<< HEAD private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.65) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); @@ -93,10 +128,19 @@ public final class Constants { public static void getIntakeWheelsSpeed() { INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.65); +======= + private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000) + .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + public static double INTAKE_WHEELS_MOTOR_RPM; + + public static void getIntakeWheelsSpeed() { + INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } public static final int INTAKE_WHEELS_MOTOR_ID = 50; public static final int INTAKE_ROTATOR_MOTOR_ID = 51; +<<<<<<< HEAD public static final double INTAKE_COLLECT_ENCODER_VALUE = 0; public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; @@ -104,12 +148,23 @@ public final class Constants { public static final double INTAKE_ROTATOR_P = 0.01; public static final double INTAKE_ROTATOR_I = 0; public static final double INTAKE_ROTATOR_D = 0; +======= + public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744; + public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; + + public static class IntakeRotatorPID { + public static final double INTAKE_ROTATOR_P = 0.05; + public static final double INTAKE_ROTATOR_I = 0; + public static final double INTAKE_ROTATOR_D = 0.001; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } } public static class RampConstants { public static final int RAMP_MOTOR_ID = 45; +<<<<<<< HEAD public static double RAMP_MOTOR_SPEED; // create object and a new widget under programming tab in Elastic where object @@ -122,6 +177,13 @@ public final class Constants { public static void getRampMotorSpeed() { RAMP_MOTOR_SPEED = rampSpeed.getDouble(0.4); } +======= + public static double RAMP_MOTOR_SPEED = .6; + + // create object and a new widget under programming tab in Elastic where object + // retrieves value from widget + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } public static class LimeLight { @@ -151,11 +213,19 @@ public final class Constants { public static class ClimberConstants { public static final int CLIMB_MOTOR_ID = 60; +<<<<<<< HEAD public static final int RATCHET_PWM_PORT = 1; public static final double RATCHET_UNLOCK_ANGLE = 0.3; public static final double RATCHET_LOCK_ANGLE = 0.15; public static final double CLIMBER_SPEED = .1; +======= + public static final int RATCHET_PWM_PORT = 0; + + public static final double RATCHET_UNLOCK_ANGLE = 0; + public static final double RATCHET_LOCK_ANGLE = 180; + public static final double CLIMBER_SPEED = .5; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index acc2cec..cc84990 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -89,11 +89,19 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); +<<<<<<< HEAD Constants.RampConstants.getRampMotorSpeed(); Constants.IntakeConstants.getIntakeWheelsSpeed(); Constants.ShooterConstants.getShooterVelocity(); Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0); distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag()); +======= + // Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); + Constants.IntakeConstants.getIntakeWheelsSpeed(); + Constants.ShooterConstants.getShooterVelocity(); + Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0); + //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag()); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b97d53..15a0753 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,10 @@ import java.util.function.DoubleSupplier; import frc.robot.subsystems.TargetingSubsystems; import swervelib.SwerveInputStream; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +<<<<<<< HEAD +======= +import frc.robot.subsystems.ClimberSubsystem; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f /** * This class is where the bulk of the robot should be declared. Since @@ -57,9 +61,18 @@ public class RobotContainer { // SmartDashboard, allowing selection of desired auto private final SendableChooser autoChooser; +<<<<<<< HEAD private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); +======= + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + // private final TargetingSubsystems m_TargetingSubsystems = new + // TargetingSubsystems(); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f /** * Converts driver input into a field-relative ChassisSpeeds that is controlled * by angular velocity. @@ -142,6 +155,7 @@ public class RobotContainer { } +<<<<<<< HEAD /** * Use this method to define your trigger->command mappings. Triggers can be * created via the @@ -234,6 +248,116 @@ public class RobotContainer { } } +======= + /** + * Use this method to define your trigger->command mappings. Triggers can be + * created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with + * an arbitrary predicate, or via the + * named factories in + * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses + * for + * {@link CommandXboxController + * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} + * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick + * Flight joysticks}. + */ + private void configureBindings() { + Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); + Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngle); + Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); + Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); + Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngleKeyboard); + + driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand()) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); + driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); + + // command for + // full shooting system including linear actuators + driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand() + .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); + + driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + + driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); + driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); + driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); + driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); + + + // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); + driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand())); + // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), + // () -> -driverXbox.getLeftX())); + if (driverXbox.getRightY() < -0.4) { + m_ClimberSubsystem.liftRobotCommand(); + } else if (driverXbox.getRightY() > 0.4) { + m_ClimberSubsystem.lowerRobotCommand(); + } else { + m_ClimberSubsystem.stopClimberCommand(); + } + + // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE)); + + if (RobotBase.isSimulation()) { + drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); + } else { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + } + + if (Robot.isSimulation()) { + Pose2d target = new Pose2d(new Translation2d(1, 4), + Rotation2d.fromDegrees(90)); + // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); + driveDirectAngleKeyboard.driveToPose(() -> target, + new ProfiledPIDController(5, + 0, + 0, + new Constraints(5, 2)), + new ProfiledPIDController(5, + 0, + 0, + new Constraints(Units.degreesToRadians(360), + Units.degreesToRadians(180)))); + driverXbox.start() + .onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); + driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); + driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true), + () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); + + // driverXbox.b().whileTrue( + // drivebase.driveToPose( + // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + // ); + + } + if (DriverStation.isTest()) { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above! + + driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.back().whileTrue(drivebase.centerModulesCommand()); + driverXbox.leftBumper().onTrue(Commands.none()); + driverXbox.rightBumper().onTrue(Commands.none()); + } else { + driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); + driverXbox.start().whileTrue(Commands.none()); + driverXbox.back().whileTrue(Commands.none()); + driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.rightBumper().onTrue(Commands.none()); + } + + } +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -250,6 +374,7 @@ public class RobotContainer { drivebase.setMotorBrake(brake); } +<<<<<<< HEAD public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) { try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) { aimPIDs.setTolerance(1.0); @@ -284,4 +409,10 @@ public class RobotContainer { public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( m_shooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), m_shooterSubsystem.shootFuelCommand()); +======= + public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( + // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), + m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index a3151c0..6c68745 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -3,6 +3,10 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.Servo; +<<<<<<< HEAD +======= +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -37,8 +41,24 @@ public class ClimberSubsystem extends SubsystemBase{ public static void toggleRatchet(boolean toggle) { if (toggle == true) { +<<<<<<< HEAD climberRatchet.set(Constants.ClimberConstants.RATCHET_LOCK_ANGLE); } else climberRatchet.set(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE); +======= + climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE); + } else + climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE); + } + + public Command toggleRatchetCommand(boolean toggle) { + return runOnce(() -> toggleRatchet(toggle)); + } + + @Override + public void periodic() + { + SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition()); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 88e2429..a61659e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,3 +1,4 @@ +<<<<<<< HEAD package frc.robot.subsystems; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -78,3 +79,109 @@ public class IntakeSubsystem extends SubsystemBase { // Shuffleboard.getTab("Intake Rotator Motor").add("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); } } +======= +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; + +public class IntakeSubsystem extends SubsystemBase { + + private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, + MotorType.kBrushless); + private static SparkClosedLoopController intakeRotatorPIDController; + public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController intakeWheelsMotorPIDController; + public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig(); + + public IntakeSubsystem() { + intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D); + intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + + intakeWheelsMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); + } + + public void startIntakeMotor() { + intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity); + } + + public void reverseIntakeMotor() { + intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity); + } + + public void stopIntakeMotor() { + intakeWheelsMotor.set(0); + } + + public Command startIntakeMotorCommand() { + return runOnce(() -> startIntakeMotor()); + } + + public Command reverseIntakeMotorCommand() { + return runOnce(() -> reverseIntakeMotor()); + } + + public Command stopIntakeMotorCommand() { + return runOnce(() -> stopIntakeMotor()); + } + + public void deployIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, + ControlType.kPosition); + } + + public Command deployintakeCommand() { + return runOnce(() -> deployIntake()); + } + + public void retractIntake() { + intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition); + } + + public Command retractIntakeCommand() { + return runOnce(() -> retractIntake()); + } + + public void assistFuelIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, + ControlType.kPosition); + } + + public Command assistFuelIntakeCommand() { + return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand()) + .andThen(new WaitCommand(2)); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); + } +} +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7bcde9c..945aa8a 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,3 +1,4 @@ +<<<<<<< HEAD package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -119,3 +120,173 @@ public class ShooterSubsystem extends SubsystemBase { currentPotentiometerPosition = leftPotentiometer.get(); } } +======= +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import java.util.function.BooleanSupplier; + +import com.ctre.phoenix6.controls.Follower; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.LimelightHelpers; +import edu.wpi.first.wpilibj.AnalogPotentiometer; + +public class ShooterSubsystem extends SubsystemBase { + private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkClosedLoopController centerShooterMotorPIDController; + public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController leftShooterMotorPIDController; + public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController rightShooterMotorPIDController; + public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController indexerAndRampMotorPIDController; + public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig(); + + public ShooterSubsystem() { + centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController(); + + leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController(); + + rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController(); + + indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController(); + } + + + //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, + // MotorType.kBrushless); + + //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, + //MotorType.kBrushless); + + //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0); + //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0); + + private static double currentPotentiometerPosition; // might need second value for the right potentiometer + + public void setShooterMotorsRPM() { + centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + } + + public double getShooterMotorRPM() { + return leftShooterMotor.getEncoder().getVelocity(); + } + + public void setIndexerAndRampMotorRPM() { + // if (LimelightHelpers.getTX("limelight") < 1.5 && + // LimelightHelpers.getTX("limelight") > -1.5) { + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity); + // } else + // indexerMotor.set(0); + } + + /* public Command shootFuelCommand() { + return run(() -> startShooterMotors()) + .until(() -> { + return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY); + }) + .andThen(() -> startIndexerMotor()); + } */ + + + public Command shootFuelCommand() { + return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) + .andThen(() -> setIndexerAndRampMotorRPM()); + }; + + + public void stopShooters() { + centerShooterMotor.set(0); + leftShooterMotor.set(0); + rightShooterMotor.set(0); + indexerAndRampMotor.set(0); + } + + public Command stopShooterCommand() { + return runOnce(() -> stopShooters()); + } + + public void moveActuator(double desiredPotentiometerPosition) { + if (desiredPotentiometerPosition > currentPotentiometerPosition) { + //TODO: Test for positive or negative power + //leftActuatorMotor.set(0.1); + //rightActuatorMotor.set(0.1); + } else { + //leftActuatorMotor.set(-0.1); + //rightActuatorMotor.set(-0.1); + } + } + + public void stopActuator() { + //leftActuatorMotor.set(0); + //rightActuatorMotor.set(0); + } + + public Command moveActuatorCommand(double desiredPotentiometerPosition) { + return run(() -> moveActuator(desiredPotentiometerPosition)) + .until(() -> currentPotentiometerPosition == currentPotentiometerPosition) + .andThen(() -> stopActuator()); + } + + @Override + public void periodic() { + /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); + SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get()); + currentPotentiometerPosition = leftPotentiometer.get(); */ + } +} +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index 128bb4c..0b0604e 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -38,6 +38,10 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +<<<<<<< HEAD +======= +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -45,9 +49,13 @@ import frc.robot.RobotContainer; import frc.robot.Constants; public class TargetingSubsystems extends SubsystemBase { +<<<<<<< HEAD RobotContainer m_RobotContainer = new RobotContainer(); +======= + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera"); Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator( @@ -63,10 +71,17 @@ public class TargetingSubsystems extends SubsystemBase { public List rightClimbWaypoints; +<<<<<<< HEAD public Command pathPlanToRightClimbPoseCommand() { GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation()); PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0); currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose(); +======= + public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) { + GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation()); + PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0); + currentRobotPose = swerveDrive.getPose(); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f rightClimbWaypoints = PathPlannerPath.waypointsFromPoses( currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE); @@ -74,12 +89,21 @@ public class TargetingSubsystems extends SubsystemBase { goalEndState); goToClimbPath.preventFlipping = true; +<<<<<<< HEAD return m_RobotContainer.getSwerveDriveBase().getAutonomousCommand("goToClimbPath"); } public Command aimAndRangeToPose(Pose2d desiredPose) { return new RunCommand(() -> { currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose(); +======= + return swerveDrive.getAutonomousCommand("goToClimbPath"); + } + + public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) { + return new RunCommand(() -> { + currentRobotPose = swerveDrive.getPose(); +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); @@ -98,11 +122,20 @@ public class TargetingSubsystems extends SubsystemBase { double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(), desiredPose.getRotation().getRadians()); +<<<<<<< HEAD m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true); }, m_RobotContainer.getSwerveDriveBase()); } Command photonAimAtClimb = new RunCommand(() -> { +======= + swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true); + }, swerveDrive); + } + + Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { + return new RunCommand(() -> { +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f double rot = 0.0; var result = photonVision.getLatestResult(); if (result.hasTargets()) { @@ -112,9 +145,16 @@ public class TargetingSubsystems extends SubsystemBase { rot = MathUtil.clamp(rot, -3.0, 3.0); +<<<<<<< HEAD m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(m_RobotContainer.getDriverXbox().getLeftY() * -1, m_RobotContainer.getDriverXbox().getLeftX() * -1), rot, true); }, m_RobotContainer.getSwerveDriveBase()); +======= + swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1, + driverXbox.getLeftX() * -1), rot, true); + }, swerveDrive); +} +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f public PhotonPoseEstimator getPhotonPoseEstimator() { @@ -150,6 +190,7 @@ public class TargetingSubsystems extends SubsystemBase { return distanceFromLimelightToGoalInches; } +<<<<<<< HEAD @Override public void periodic() { @@ -160,6 +201,20 @@ public class TargetingSubsystems extends SubsystemBase { m_RobotContainer.getSwerveDriveBase().getSwerveDrive() .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); } +======= + public void updateRobotPose(SwerveSubsystem swerveDrive) { + Optional result = photonEstimator.update(photonVision.getLatestResult()); + + if (result.isPresent()) { + EstimatedRobotPose estimatedPose = result.get(); + swerveDrive.getSwerveDrive() + .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); + } + } + @Override + public void periodic() { + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f /* * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", * photonVision.getLatestResult().getBestTarget().getYaw()); @@ -174,5 +229,11 @@ public class TargetingSubsystems extends SubsystemBase { * "Arducam_OV9281_USB_Camera", * "http://photonvision.local:5800"); */ +<<<<<<< HEAD } } +======= + } +} + +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index d677507..7168ff3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -62,7 +62,11 @@ public class SwerveSubsystem extends SubsystemBase { /** * Enable vision odometry updates while driving. */ +<<<<<<< HEAD private final boolean visionDriveTest = true; +======= + private final boolean visionDriveTest = false; +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f /** * PhotonVision class to keep an accurate odometry. diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..5e93833 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,11 @@ { "fileName": "REVLib.json", "name": "REVLib", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +16,22 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", +<<<<<<< HEAD "version": "2026.0.1" +======= + "version": "2026.0.2" +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +46,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +65,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +86,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +107,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +128,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +148,11 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", +<<<<<<< HEAD "version": "2026.0.1", +======= + "version": "2026.0.2", +>>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true,