From 93787339714d39de8d27d097fa758d6b8b47fe8e Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Wed, 28 Jan 2026 18:36:14 -0600 Subject: [PATCH 01/19] DTM-520 Test --- 2026_Rebuilt/src/main/java/frc/robot/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index c50ba05..c9be055 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -16,4 +16,5 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + /// GIT HUB TEST } From 24f8054ac4e25bfed346bb8ed3e212e9dd4dee0b Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Wed, 28 Jan 2026 18:38:55 -0600 Subject: [PATCH 02/19] DTM Test 2 --- 2026_Rebuilt/src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index c9be055..e755482 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -16,5 +16,5 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } - /// GIT HUB TEST + /// GIT HUB TEST!!!!!!!!!!! } From b3c0021ce65e82e1d70d79f3d89e755893a86b22 Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Mon, 2 Feb 2026 20:25:18 -0600 Subject: [PATCH 03/19] Swerve Programmed Swerve should be completed, testing is needed. Waiting till Wednesday to test --- .../src/main/java/frc/robot/Constants.java | 51 ++++- .../src/main/java/frc/robot/Robot.java | 13 +- .../main/java/frc/robot/RobotContainer.java | 40 ++-- .../java/frc/robot/Swerve/SwerveModule.java | 147 ++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 7 + .../robot/subsystems/ShooterSubsystem.java | 7 + .../subsystems/SwerveDrivetrainSubsystem.java | 181 ++++++++++++++++++ 2026_Rebuilt/vendordeps/REVLib.json | 133 +++++++++++++ 2026_Rebuilt/vendordeps/Studica.json | 71 +++++++ 9 files changed, 628 insertions(+), 22 deletions(-) create mode 100644 2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java create mode 100644 2026_Rebuilt/vendordeps/REVLib.json create mode 100644 2026_Rebuilt/vendordeps/Studica.json diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index e755482..34b4900 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -4,6 +4,8 @@ package frc.robot; +import edu.wpi.first.math.util.Units; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -11,10 +13,51 @@ * *

It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. - */ + **/ public final class Constants { - public static class OperatorConstants { + + + //Controllers public static final int kDriverControllerPort = 0; - } - /// GIT HUB TEST!!!!!!!!!!! + public static final int kFunctionsControllerPort = 1; + + //Drive Motors + public static final int kFrontLeftDrive = 5; + public static final int kFrontRightDrive = 3; + public static final int kBackLeftDrive = 9; + public static final int kBackRightDrive = 7; + + + //Steering Motors + public static final int kFrontLeftSteering = 4; + public static final int kFrontRightSteering = 2; + public static final int kBackLeftSteering = 8; + public static final int kBackRightSteering = 6; + + + //Encoders + + public static final int kFrontLeftEncoder = 1; + public static final int kFrontRightEncoder = 0; + public static final int kBackLeftEncoder = 2; + public static final int kBackRightEncoder = 3; + + //Encoder Offset + public static final double kFrontLeftEncoderOffset = 0.15; + public static final double kFrontRightEncoderOffset = -0.21; + public static final double kBackLeftEncoderOffset = 0.25; + public static final double kBackRightEncoderOffset = 0.07; + + + //Dampners + public static final double kSwerveDampner = 0.50; + public static final double kElevatorDampner = 0.5; + public static final double kClimbDampner = 0.5; + public static final double kAlgaeDampner = 0.2; + + + //Chasis Length + public static final double chasisWidth = Units.inchesToMeters(20); + public static final double chasisLength = Units.inchesToMeters(24); + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/Robot.java b/2026_Rebuilt/src/main/java/frc/robot/Robot.java index 9abb975..68ee620 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Robot.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Robot.java @@ -54,12 +54,15 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); - } + + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // // schedule the autonomous command (example) + // if (m_autonomousCommand != null) { + // CommandScheduler.getInstance().schedule(m_autonomousCommand); + // } + } /** This function is called periodically during autonomous. */ diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index a33249e..5d21ce0 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,16 @@ package frc.robot; -import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants; import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.SwerveDrivetrainSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -19,17 +24,23 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + + // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + private final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem(); // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + private final CommandJoystick m_driverController = + new CommandJoystick(Constants.kDriverControllerPort); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + + swerveDrivetrainSubsystem.setDefaultCommand(new RunCommand(()-> this.swerveDrivetrainSubsystem.driveSwerve(this.m_driverController), swerveDrivetrainSubsystem )); } /** @@ -43,12 +54,13 @@ public RobotContainer() { */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + // new Trigger(m_exampleSubsystem::exampleCondition) + // .onTrue(new ExampleCommand(m_exampleSubsystem)); + + // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // // cancelling on release. + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } /** @@ -56,8 +68,10 @@ private void configureBindings() { * * @return the command to run in autonomous */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); - } + + + // public Command getAutonomousCommand() { + // // An example command will be run in autonomous + // // return Autos.exampleAuto(m_exampleSubsystem); + // } } diff --git a/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java b/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java new file mode 100644 index 0000000..44787ae --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java @@ -0,0 +1,147 @@ +package frc.robot.Swerve; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogEncoder; +import frc.robot.Constants; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; + + +public class SwerveModule { + + + //Motors + public SparkMax steeringMotor; + public SparkFlex driveMotor; + + //Modules States + public SwerveModuleState moduleState; + public SwerveModulePosition modulePosition; + + //Encoder + public AnalogEncoder encoder; + public RelativeEncoder driveEncoder; + public double encoderValue; + public double encoderOffset; + + //PID + public PIDController pidController; + public double distanceMoved = 0; + public double endpoint; + public double pidSpeed; + public double errorValue; + public Rotation2d Angle = Rotation2d.fromDegrees(0); + + //Modifiers + private int steeringModifier = 1; + private int driveModifier = 1; + + + + + + public SwerveModule(int driveMotorPort,int steeringMotorPort, int encoderPort, double offset, boolean steeringModified, boolean driveModified) + { + + //Motors + steeringMotor = new SparkMax(steeringMotorPort, MotorType.kBrushless); + driveMotor = new SparkFlex(driveMotorPort, MotorType.kBrushless); + + //Module State + moduleState = new SwerveModuleState(); + modulePosition = new SwerveModulePosition(0,Rotation2d.fromDegrees(0)); + + + //Encoder + encoder = new AnalogEncoder(encoderPort); + driveEncoder = driveMotor.getEncoder(); + + + //PID + //original 3 + pidController = new PIDController(3, 0, 0); + pidController.enableContinuousInput(0, 1); + pidController.setTolerance(0.001); + + //Offset + encoderOffset = offset; + + //Invert + if (steeringModified == true){ + + steeringModifier = -1; + } + + if (driveModified == true){ + + driveModifier = -1; + } + + + + + + }; + + + + /** + public SwerveModuleState getModuleState(){ + return moduleState; + } + public SwerveModulePosition getModulePosition(){ + return modulePosition = new SwerveModulePosition(distanceMoved, Angle); + } + */ + + public void setModuleState(SwerveModuleState state){ + + + moduleState = state; + + + //Offset Calculations + encoderValue = (this.encoder.get()+encoderOffset); + if(encoderValue > 1){ + encoderValue -= 1; + } + if (encoderValue < 0 ){ + encoderValue += 1; + } + + + moduleState.optimize(Rotation2d.fromRadians((encoderValue/1)*(Math.PI*2))); + + + endpoint = (moduleState.angle.getRadians()/(Math.PI*2)); + pidSpeed = pidController.calculate(encoderValue, endpoint); + errorValue = 1 - Math.abs(pidController.getError()); + + if (errorValue <= pidController.getErrorTolerance()){ + pidSpeed = 0; + } + + //Set Steering Speed and Driving Speed + steeringMotor.set(steeringModifier*pidSpeed); + driveMotor.set(driveModifier*moduleState.speedMetersPerSecond); + + + + } + + public void setModulePosition(){ + + distanceMoved = driveEncoder.getPosition()*(Units.inchesToMeters(4)*Math.PI)/(6.75); + + Angle = Rotation2d.fromRadians((encoder.get()/1)*(Math.PI*2)); + } + +} + diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..b88bbd6 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakeSubsystem extends SubsystemBase{ + +} diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..dc5be87 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase{ + +} diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java new file mode 100644 index 0000000..3a25199 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java @@ -0,0 +1,181 @@ +// 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.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Kinematics; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PS5Controller; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.Swerve.SwerveModule; + + +/** Add your docs here. */ +public class SwerveDrivetrainSubsystem extends SubsystemBase{ + + + SwerveModule frontRightModule = new SwerveModule(Constants.kFrontRightDrive, Constants.kFrontRightSteering, Constants.kFrontRightEncoder, Constants.kFrontRightEncoderOffset, true, false); + SwerveModule frontLeftModule = new SwerveModule(Constants.kFrontLeftDrive, Constants.kFrontLeftSteering,Constants.kFrontLeftEncoder, Constants.kFrontLeftEncoderOffset, false, false); + SwerveModule backRightModule = new SwerveModule(Constants.kBackRightDrive, Constants.kBackRightSteering,Constants.kBackRightEncoder, Constants.kBackRightEncoderOffset, false, false); + SwerveModule backLeftModule = new SwerveModule(Constants.kBackLeftDrive, Constants.kBackLeftSteering, Constants.kBackLeftEncoder, Constants.kBackLeftEncoderOffset, true, true); + + + SwerveModuleState states[]; + SwerveModulePosition[] position = {frontLeftModule.modulePosition.copy(), frontRightModule.modulePosition.copy(), backLeftModule.modulePosition.copy(), backRightModule.modulePosition.copy()}; + + + AHRS Navx = new AHRS(NavXComType.kMXP_SPI); + + // Uncomment to convert from double to Rotations2D + Rotation2d Yaw; + PIDController ResetToFusedHeading = new PIDController(10, 0, 0); + + Pose2d initialrobotPose2d = new Pose2d(); + + + Translation2d frontLeft = new Translation2d((Constants.chasisWidth/2), (Constants.chasisLength/2)); + Translation2d frontRight = new Translation2d((Constants.chasisWidth/2), (-Constants.chasisLength/2)); + Translation2d backLeft = new Translation2d((-Constants.chasisWidth/2), (Constants.chasisLength/2)); + Translation2d backRight = new Translation2d((-Constants.chasisWidth/2), (-Constants.chasisLength/2)); + + Rotation2d FLCurrentAngle; + Rotation2d FRCurrentAngle; + Rotation2d BLCurrentAngle; + Rotation2d BRCurrentAngle; + + + SwerveDriveKinematics kinematics = new SwerveDriveKinematics(frontLeft, frontRight, backLeft, backRight); + + SwerveDrivePoseEstimator swerveDrivePoseEstimator; + + double distanceAprilTag; + + + + static ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + + + public SwerveDrivetrainSubsystem(){ + + + + } + + // public double getCurrentYaw(){ + + // Yaw = Navx.getYaw(); + // return Yaw; + + // } + + + + + public void driveSwerve(CommandJoystick driveController ){ + + Yaw = Navx.getRotation2d(); + + double velocityX = -1 * driveController.getY(); + double velocityY = 1 * driveController.getX(); + double omega = 1 * driveController.getZ(); + + + if(Math.abs(velocityX) < 0.1) velocityX = 0; + if(Math.abs(velocityY) < 0.1) velocityY = 0; + if(Math.abs(omega) < 0.1) omega = 0; + + + + System.out.println("Velocity X: " + velocityX); + System.out.println("Velocity Y: "+ velocityY); + System.out.println("Omega: "+omega); + System.out.println("FUSEDHEADING"+Navx.getFusedHeading()+"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); + + + chassisSpeeds = new ChassisSpeeds(); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(velocityX,velocityY, omega, Yaw); + + setSpeed(chassisSpeeds); + + + position[0] = frontLeftModule.modulePosition; + position[1] = frontRightModule.modulePosition; + position[2] = backLeftModule.modulePosition; + position[3] = backRightModule.modulePosition; + + + // swerveDrivePoseEstimator.update(getCurrentYaw(), updatePositions()); + + // SmartDashboard.putNumber("Front Left Module Enocoder", frontLeftModule.encoderValue); + // SmartDashboard.putNumber("Front Right Module Enocoder", frontRightModule.encoderValue); + // SmartDashboard.putNumber("Back Left Module Enocoder", backLeftModule.encoderValue); + // SmartDashboard.putNumber("Back Right Module Enocoder", backRightModule.encoderValue); + + + + } + + + + public void setSpeed(ChassisSpeeds speed){ + // System.out.println("Setting states " ); + System.out.println("Speed: "+speed); + states = kinematics.toSwerveModuleStates(speed); + + + // System.out.println("Setting states "+states ); + frontLeftModule.setModuleState(states[0]); + frontRightModule.setModuleState(states[1]); + backLeftModule.setModuleState(states[2]); + backRightModule.setModuleState(states[3]); + + //Printing out yaw + // SmartDashboard.putNumber("YAW", Yaw); + + + // System.out.println(); + // System.out.println("Encoder Value: "+backLeftModule.encoderValue); + // System.out.println("Endpoint: "+backLeftModule.endpoint); + // System.out.println("PID speed: "+backLeftModule.pidSpeed); + // System.out.println("Error Tolerance: "+ backLeftModule.pidController.getErrorTolerance()); + // System.out.println(); + frontLeftModule.setModulePosition(); + frontRightModule.setModulePosition(); + backLeftModule.setModulePosition(); + backRightModule.setModulePosition(); + + // swerveDrivePoseEstimator.update(getCurrentYaw(), updatePositions()); + System.out.println("Fused Heading "+Navx.getFusedHeading()+" !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); + System.out.println("Current Yaw: "+ Navx.getYaw()+" !!!!!!!!!!!!!"); + + + } + + public SwerveDriveKinematics getSwerveKinematics(){ + return this.getSwerveKinematics(); + } +} \ No newline at end of file diff --git a/2026_Rebuilt/vendordeps/REVLib.json b/2026_Rebuilt/vendordeps/REVLib.json new file mode 100644 index 0000000..d35e593 --- /dev/null +++ b/2026_Rebuilt/vendordeps/REVLib.json @@ -0,0 +1,133 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.1", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.1", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.1", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/2026_Rebuilt/vendordeps/Studica.json b/2026_Rebuilt/vendordeps/Studica.json new file mode 100644 index 0000000..b51bf58 --- /dev/null +++ b/2026_Rebuilt/vendordeps/Studica.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica.json", + "name": "Studica", + "version": "2026.0.0", + "frcYear": "2026", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2026/" + ], + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/Studica-2026.0.0.json", + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-java", + "version": "2026.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-cpp", + "version": "2026.0.0", + "libName": "Studica", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "libName": "StudicaDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From fecc215d0c8bfa58ff420035268b58eb33aade37 Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Wed, 4 Feb 2026 20:20:02 -0600 Subject: [PATCH 04/19] Update CAN IDs --- 2026_Rebuilt/src/main/java/frc/robot/Constants.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 34b4900..915834f 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -21,6 +21,9 @@ public final class Constants { public static final int kDriverControllerPort = 0; public static final int kFunctionsControllerPort = 1; + //Intake + public static final int KIntakeMotor = 10; + //Drive Motors public static final int kFrontLeftDrive = 5; public static final int kFrontRightDrive = 3; From 72dcbe24c8dfc3e666a66a872a07f18c8c664df8 Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Wed, 11 Feb 2026 12:02:17 -0600 Subject: [PATCH 05/19] Skeleton Done Skeleton Done, Testing needed. Implemented Shooter, Intake, Drive, and Transfer through RobotContainer --- .../main/java/frc/robot/RobotContainer.java | 20 +++++++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 14 +++++++++++++ .../robot/subsystems/ShooterSubsystem.java | 16 +++++++++++++++ .../robot/subsystems/TransferSubsystem.java | 17 ++++++++++++++++ 4 files changed, 67 insertions(+) create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index 5d21ce0..1292656 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,10 @@ import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDrivetrainSubsystem; +import frc.robot.subsystems.TransferSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; @@ -28,17 +31,29 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem(); + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + private final TransferSubsystem transferSubsystem = new TransferSubsystem(); + + private final RunCommand shoot = new RunCommand(()-> this.shooterSubsystem.shoot(), shooterSubsystem); + private final RunCommand intake = new RunCommand(()-> this.intakeSubsystem.intake(), intakeSubsystem); + private final RunCommand outake = new RunCommand(()-> this.intakeSubsystem.outake(), intakeSubsystem); + private final RunCommand transfer = new RunCommand(()-> this.transferSubsystem.transfer(), transferSubsystem); + + // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandJoystick m_driverController = new CommandJoystick(Constants.kDriverControllerPort); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + swerveDrivetrainSubsystem.setDefaultCommand(new RunCommand(()-> this.swerveDrivetrainSubsystem.driveSwerve(this.m_driverController), swerveDrivetrainSubsystem )); } @@ -53,6 +68,11 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + m_driverController.button(1).onTrue(shoot); + m_driverController.button(2).onTrue(intake); + m_driverController.button(3).onTrue(outake); + m_driverController.button(4).onTrue(transfer); + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) // .onTrue(new ExampleCommand(m_exampleSubsystem)); diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index b88bbd6..b35cd2e 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,7 +1,21 @@ package frc.robot.subsystems; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase{ + private SparkMax intakeRoller = new SparkMax(0, MotorType.kBrushless); + + + public void intake(){ + intakeRoller.set(1); + } + + public void outake(){ + intakeRoller.set(-1); + } + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index dc5be87..d2a1b2e 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,7 +1,23 @@ package frc.robot.subsystems; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.SubsystemBase; + public class ShooterSubsystem extends SubsystemBase{ + private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); + private SparkMax backRollers = new SparkMax(0, MotorType.kBrushless); + private SparkMax feedRollers = new SparkMax(0, MotorType.kBrushless); + + + public void shoot(){ + + flywheel.set(0.5); + backRollers.set(0.5); + feedRollers.set(0.5); + } + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java new file mode 100644 index 0000000..be6a469 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +public class TransferSubsystem extends SubsystemBase{ + + + private SparkMax transfer = new SparkMax(0, MotorType.kBrushless); + + + public void transfer(){ + transfer.set(1); + } + +} From 3201e7811296b3f7e70fd5a4ce38902ebe4940ae Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Thu, 12 Feb 2026 20:13:32 -0600 Subject: [PATCH 06/19] Add CAN ID constants --- 2026_Rebuilt/src/main/java/frc/robot/Constants.java | 6 +++++- .../src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../java/frc/robot/subsystems/IntakeSubsystem.java | 3 ++- .../java/frc/robot/subsystems/ShooterSubsystem.java | 10 ++++++---- .../java/frc/robot/subsystems/TransferSubsystem.java | 3 ++- 5 files changed, 18 insertions(+), 10 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 915834f..b8577a7 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -22,7 +22,10 @@ public final class Constants { public static final int kFunctionsControllerPort = 1; //Intake - public static final int KIntakeMotor = 10; + public static final int KIntakeMotor = 11; + public static final int KFlywheelMotor = 15; + public static final int KBackRollerMotor = 10; + //Drive Motors public static final int kFrontLeftDrive = 5; @@ -38,6 +41,7 @@ public final class Constants { public static final int kBackRightSteering = 6; + //Encoders public static final int kFrontLeftEncoder = 1; diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index 1292656..777d283 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -68,10 +68,10 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - m_driverController.button(1).onTrue(shoot); + m_driverController.button(6).onTrue(shoot); m_driverController.button(2).onTrue(intake); - m_driverController.button(3).onTrue(outake); - m_driverController.button(4).onTrue(transfer); + m_driverController.button(4).onTrue(outake); + m_driverController.button(5).onTrue(transfer); // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index b35cd2e..c2f3c8e 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,10 +4,11 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class IntakeSubsystem extends SubsystemBase{ - private SparkMax intakeRoller = new SparkMax(0, MotorType.kBrushless); + private SparkMax intakeRoller = new SparkMax(Constants.KIntakeMotor, MotorType.kBrushless); public void intake(){ diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d2a1b2e..e776b73 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,23 +1,25 @@ package frc.robot.subsystems; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class ShooterSubsystem extends SubsystemBase{ - private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); - private SparkMax backRollers = new SparkMax(0, MotorType.kBrushless); - private SparkMax feedRollers = new SparkMax(0, MotorType.kBrushless); + private SparkFlex flywheel = new SparkFlex(Constants.KFlywheelMotor, MotorType.kBrushless); + private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); public void shoot(){ flywheel.set(0.5); backRollers.set(0.5); - feedRollers.set(0.5); + // feedRollers.set(0.5); } } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java index be6a469..6d7d89a 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java @@ -7,7 +7,7 @@ public class TransferSubsystem extends SubsystemBase{ - private SparkMax transfer = new SparkMax(0, MotorType.kBrushless); + private SparkMax transfer = new SparkMax(14, MotorType.kBrushless); public void transfer(){ @@ -15,3 +15,4 @@ public void transfer(){ } } + \ No newline at end of file From d73c9c8d6cb502cc90c212dd55d7627849347886 Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Sat, 14 Feb 2026 14:21:20 -0600 Subject: [PATCH 07/19] Skeletons Tested All the Subsystems are tested except Transfer. Started working on photon Pose estimator --- .../src/main/java/frc/robot/Constants.java | 2 +- .../main/java/frc/robot/RobotContainer.java | 15 ++-- .../frc/robot/subsystems/IntakeSubsystem.java | 8 ++- .../subsystems/PoseEstimatorSubsystem.java | 37 ++++++++++ .../robot/subsystems/ShooterSubsystem.java | 11 ++- .../subsystems/SwerveDrivetrainSubsystem.java | 3 +- 2026_Rebuilt/vendordeps/photonlib.json | 71 +++++++++++++++++++ 7 files changed, 136 insertions(+), 11 deletions(-) create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java create mode 100644 2026_Rebuilt/vendordeps/photonlib.json diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index b8577a7..383f178 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -57,7 +57,7 @@ public final class Constants { //Dampners - public static final double kSwerveDampner = 0.50; + public static final double kSwerveDampner = 0.05; public static final double kElevatorDampner = 0.5; public static final double kClimbDampner = 0.5; public static final double kAlgaeDampner = 0.2; diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index 777d283..2dcd4c3 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import frc.robot.subsystems.SwerveDrivetrainSubsystem; import frc.robot.subsystems.TransferSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; @@ -30,7 +31,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem(); + public final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem(); private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); private final TransferSubsystem transferSubsystem = new TransferSubsystem(); @@ -39,11 +40,14 @@ public class RobotContainer { private final RunCommand intake = new RunCommand(()-> this.intakeSubsystem.intake(), intakeSubsystem); private final RunCommand outake = new RunCommand(()-> this.intakeSubsystem.outake(), intakeSubsystem); private final RunCommand transfer = new RunCommand(()-> this.transferSubsystem.transfer(), transferSubsystem); + private final InstantCommand stop = new InstantCommand(()-> this.shooterSubsystem.stop(), shooterSubsystem); + private final InstantCommand stopIntake = new InstantCommand(()-> this.intakeSubsystem.stopIntake(), intakeSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandJoystick m_driverController = + public static final CommandJoystick m_driverController = new CommandJoystick(Constants.kDriverControllerPort); @@ -55,7 +59,7 @@ public RobotContainer() { configureBindings(); - swerveDrivetrainSubsystem.setDefaultCommand(new RunCommand(()-> this.swerveDrivetrainSubsystem.driveSwerve(this.m_driverController), swerveDrivetrainSubsystem )); + } /** @@ -69,9 +73,10 @@ public RobotContainer() { */ private void configureBindings() { m_driverController.button(6).onTrue(shoot); - m_driverController.button(2).onTrue(intake); + m_driverController.button(2).onTrue(stop); m_driverController.button(4).onTrue(outake); - m_driverController.button(5).onTrue(transfer); + m_driverController.button(5).onTrue(intake); + m_driverController.button(1).onTrue(stopIntake); // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c2f3c8e..f51abde 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -12,11 +12,15 @@ public class IntakeSubsystem extends SubsystemBase{ public void intake(){ - intakeRoller.set(1); + intakeRoller.set(0.5); } public void outake(){ - intakeRoller.set(-1); + intakeRoller.set(-0.5); + } + + public void stopIntake(){ + intakeRoller.set(0); } } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java new file mode 100644 index 0000000..f04bb95 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java @@ -0,0 +1,37 @@ +// 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 org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class PoseEstimatorSubsystem extends SubsystemBase { + + /** Creates a new PoseEstimatorSubsystem. */ + + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0, 0.0, 0), new Rotation3d(0, 0, 0)); + + + + public PoseEstimatorSubsystem() { + PhotonPoseEstimator poseEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam); + PhotonCamera cam = new PhotonCamera("Name"); + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e776b73..fa17c4c 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -17,9 +17,16 @@ public class ShooterSubsystem extends SubsystemBase{ public void shoot(){ - flywheel.set(0.5); - backRollers.set(0.5); + flywheel.set(-0.75); + backRollers.set(-0.75); // feedRollers.set(0.5); } + public void stop(){ + flywheel.set(0); + backRollers.set(0); + + + } + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java index 3a25199..6d8c88b 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java @@ -81,6 +81,7 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase{ public SwerveDrivetrainSubsystem(){ + this.setDefaultCommand(new RunCommand(()-> this.driveSwerve(RobotContainer.m_driverController), this )); } @@ -99,7 +100,7 @@ public void driveSwerve(CommandJoystick driveController ){ Yaw = Navx.getRotation2d(); - double velocityX = -1 * driveController.getY(); + double velocityX = 1 * driveController.getY(); double velocityY = 1 * driveController.getX(); double omega = 1 * driveController.getZ(); diff --git a/2026_Rebuilt/vendordeps/photonlib.json b/2026_Rebuilt/vendordeps/photonlib.json new file mode 100644 index 0000000..fbb5c80 --- /dev/null +++ b/2026_Rebuilt/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.2.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.2.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.2.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.2.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.2.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.2.2" + } + ] +} \ No newline at end of file From 3d3488c7bbd616f96b1776b86f7d11a8dfc0ca0f Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Mon, 23 Feb 2026 20:19:42 -0600 Subject: [PATCH 08/19] PoseEstimatorComplete - no test Pose estimator completed, testing required - Dhruv Mogenahalli --- .../src/main/java/frc/robot/Constants.java | 23 +++ .../main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/Swerve/SwerveModule.java | 4 + .../subsystems/PoseEstimatorSubsystem.java | 37 ----- .../subsystems/SwerveDrivetrainSubsystem.java | 30 +++- .../frc/robot/subsystems/VisionSubsystem.java | 134 ++++++++++++++++++ 2026_Rebuilt/vendordeps/REVLib.json | 18 +-- 7 files changed, 203 insertions(+), 48 deletions(-) delete mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java create mode 100644 2026_Rebuilt/src/main/java/frc/robot/subsystems/VisionSubsystem.java diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 383f178..c564464 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -4,6 +4,13 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; /** @@ -67,4 +74,20 @@ public final class Constants { public static final double chasisWidth = Units.inchesToMeters(20); public static final double chasisLength = Units.inchesToMeters(24); + + + + //Camera + public static final AprilTagFields kField = AprilTagFields.kDefaultField; + public static final String kCameraName = "NAME"; + public static final Transform3d kRobotToCam = new Transform3d( + Units.inchesToMeters(10), 0, Units.inchesToMeters(10), + new Rotation3d(0 , 0, 0) + ); + + + //Standard Deviations + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index 2dcd4c3..907d03b 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDrivetrainSubsystem; import frc.robot.subsystems.TransferSubsystem; +import frc.robot.subsystems.VisionSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -35,13 +36,14 @@ public class RobotContainer { private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); private final TransferSubsystem transferSubsystem = new TransferSubsystem(); + private final VisionSubsystem visionSubsystem = new VisionSubsystem(Constants.kCameraName, Constants.kField, Constants.kRobotToCam, swerveDrivetrainSubsystem::addVisionMeasurements); private final RunCommand shoot = new RunCommand(()-> this.shooterSubsystem.shoot(), shooterSubsystem); private final RunCommand intake = new RunCommand(()-> this.intakeSubsystem.intake(), intakeSubsystem); private final RunCommand outake = new RunCommand(()-> this.intakeSubsystem.outake(), intakeSubsystem); private final RunCommand transfer = new RunCommand(()-> this.transferSubsystem.transfer(), transferSubsystem); private final InstantCommand stop = new InstantCommand(()-> this.shooterSubsystem.stop(), shooterSubsystem); - private final InstantCommand stopIntake = new InstantCommand(()-> this.intakeSubsystem.stopIntake(), intakeSubsystem); + private final InstantCommand stopIntake = new InstantCommand(()-> this.intakeSubsystem.stopIntake(), intakeSubsystem); @@ -58,6 +60,7 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + visionSubsystem.periodic(); } diff --git a/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java b/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java index 44787ae..c595def 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java @@ -143,5 +143,9 @@ public void setModulePosition(){ Angle = Rotation2d.fromRadians((encoder.get()/1)*(Math.PI*2)); } + public SwerveModuleState getState() { + return this.getState(); + } + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java deleted file mode 100644 index f04bb95..0000000 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java +++ /dev/null @@ -1,37 +0,0 @@ -// 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 org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - - -public class PoseEstimatorSubsystem extends SubsystemBase { - - /** Creates a new PoseEstimatorSubsystem. */ - - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0, 0.0, 0), new Rotation3d(0, 0, 0)); - - - - public PoseEstimatorSubsystem() { - PhotonPoseEstimator poseEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam); - PhotonCamera cam = new PhotonCamera("Name"); - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java index 6d8c88b..3496475 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java @@ -7,6 +7,8 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -17,6 +19,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.DriverStation; @@ -45,6 +49,7 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase{ SwerveModuleState states[]; SwerveModulePosition[] position = {frontLeftModule.modulePosition.copy(), frontRightModule.modulePosition.copy(), backLeftModule.modulePosition.copy(), backRightModule.modulePosition.copy()}; + SwerveDrivePoseEstimator drivePoseEstimator; AHRS Navx = new AHRS(NavXComType.kMXP_SPI); @@ -72,7 +77,7 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase{ SwerveDrivePoseEstimator swerveDrivePoseEstimator; double distanceAprilTag; - + static ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); @@ -81,7 +86,17 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase{ public SwerveDrivetrainSubsystem(){ + var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + var visionStdDevs = VecBuilder.fill(1, 1, 1); + this.setDefaultCommand(new RunCommand(()-> this.driveSwerve(RobotContainer.m_driverController), this )); + drivePoseEstimator = new SwerveDrivePoseEstimator( + kinematics, + Navx.getRotation2d(), + position, + initialrobotPose2d, + stateStdDevs, + visionStdDevs); } @@ -135,6 +150,7 @@ public void driveSwerve(CommandJoystick driveController ){ // SmartDashboard.putNumber("Front Right Module Enocoder", frontRightModule.encoderValue); // SmartDashboard.putNumber("Back Left Module Enocoder", backLeftModule.encoderValue); // SmartDashboard.putNumber("Back Right Module Enocoder", backRightModule.encoderValue); + @@ -176,7 +192,19 @@ public void setSpeed(ChassisSpeeds speed){ } + public void addVisionMeasurements(Pose2d estimateRobotPose, double timestampSeconds, Matrix visionStdDevs){ + drivePoseEstimator.addVisionMeasurement(initialrobotPose2d, distanceAprilTag, visionStdDevs); + } + public SwerveDriveKinematics getSwerveKinematics(){ return this.getSwerveKinematics(); } + + + public void periodic(){ + + drivePoseEstimator.update(Navx.getRotation2d(), position); + + } + } \ No newline at end of file diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..ce69fd0 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,134 @@ +// 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 java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.List; +import org.photonvision.targeting.PhotonTrackedTarget; + + +public class VisionSubsystem extends SubsystemBase { + + /** Creates a new PoseEstimatorSubsystem. */ + + public final PhotonCamera camera; + public final PhotonPoseEstimator visionEstimator; + private Matrix curStdDevs; + private final EstimateConsumer estConsumer; + + + + + public VisionSubsystem(String cameraName, AprilTagFields tagLayout, Transform3d CamtoRobot, EstimateConsumer estimateConsumer) { + estConsumer = estimateConsumer; + AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(tagLayout); + visionEstimator = new PhotonPoseEstimator(fieldLayout, CamtoRobot); + camera = new PhotonCamera(cameraName); + + + + } + + + + @Override + public void periodic() { + Optional visionEst = Optional.empty(); + for (var result : camera.getAllUnreadResults()) { + visionEst = visionEstimator.estimateCoprocMultiTagPose(result); + if (visionEst.isEmpty()) { + visionEst = visionEstimator.estimateLowestAmbiguityPose(result); + } + updateEstimationStdDevs(visionEst, result.getTargets()); + + } + + visionEst.ifPresent( + est -> { + // Change our trust in the measurement based on the tags we can see + var estStdDevs = getEstimationStdDevs(); + + estConsumer.accept(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); + }); +} + +private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = Constants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = Constants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = visionEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = Constants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = Constants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + + +public Matrix getEstimationStdDevs(){ + return curStdDevs; +} + + +@FunctionalInterface +public static interface EstimateConsumer { + public void accept(Pose2d pose, double timestamp, Matrix estimationStdDevs); +} + +} + + diff --git a/2026_Rebuilt/vendordeps/REVLib.json b/2026_Rebuilt/vendordeps/REVLib.json index d35e593..a5af3e9 100644 --- a/2026_Rebuilt/vendordeps/REVLib.json +++ b/2026_Rebuilt/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.3", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, From 1cf1b146069153210efdb63521dcab492b4838c4 Mon Sep 17 00:00:00 2001 From: DTM-520 Date: Thu, 26 Feb 2026 20:12:51 -0600 Subject: [PATCH 09/19] Code for Saturday Test Use this code for Saturday (2/28/26) testing --- .../src/main/java/frc/robot/Constants.java | 18 +++++-- .../frc/robot/subsystems/IntakeSubsystem.java | 48 +++++++++++++++++++ .../robot/subsystems/ShooterSubsystem.java | 33 +++++++++---- .../robot/subsystems/TransferSubsystem.java | 37 +++++++++++++- 4 files changed, 121 insertions(+), 15 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index c564464..482a4d7 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -28,11 +28,22 @@ public final class Constants { public static final int kDriverControllerPort = 0; public static final int kFunctionsControllerPort = 1; + + //Shooter Motors + public static final int KFlywheelMotor_1 = 15; + public static final int KFlywheelMotor_2 = 16; + public static final int KBackRollerMotor = 10; + + //Intake public static final int KIntakeMotor = 11; - public static final int KFlywheelMotor = 15; - public static final int KBackRollerMotor = 10; + public static final int kPivotMotor = 12; + //Transfer + public static final int kTransferMotor_1 = 13; + public static final int kTransferMotor_2 = 14; + + //Drive Motors public static final int kFrontLeftDrive = 5; @@ -47,10 +58,7 @@ public final class Constants { public static final int kBackLeftSteering = 8; public static final int kBackRightSteering = 6; - - //Encoders - public static final int kFrontLeftEncoder = 1; public static final int kFrontRightEncoder = 0; public static final int kBackLeftEncoder = 2; diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index f51abde..32f16c4 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,14 +1,44 @@ package frc.robot.subsystems; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class IntakeSubsystem extends SubsystemBase{ private SparkMax intakeRoller = new SparkMax(Constants.KIntakeMotor, MotorType.kBrushless); + private SparkFlex pivotMotor = new SparkFlex(Constants.kPivotMotor, MotorType.kBrushless); + private SparkFlexConfig config = new SparkFlexConfig(); + private SparkClosedLoopController pidController = pivotMotor.getClosedLoopController(); + + public IntakeSubsystem(){ + + // config.closedLoop.p(0.1); + // config.closedLoop.i(0.0001); + // config.closedLoop.d(0.01); + // config.closedLoop.outputRange(-1, 1); + + // pivotMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + + } + + + private PIDController pid = new PIDController(3, 0, 0); + private double Tolerance = 5; + + private double extendSpeed; + private double intakeError; public void intake(){ @@ -23,4 +53,22 @@ public void stopIntake(){ intakeRoller.set(0); } + public void extendIntake(){ + + // pidController.setSetpoint(0, ControlType.kPosition); + + + pid.setTolerance(Tolerance); + extendSpeed = pid.calculate(0, 0); + pivotMotor.set(extendSpeed); + + + intakeError = 1 - Math.abs(pid.getError()); + + if (intakeError <= pid.getErrorTolerance()){ + extendSpeed = 0; + } + + } + } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index fa17c4c..660bd16 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,32 +1,49 @@ package frc.robot.subsystems; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; - +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class ShooterSubsystem extends SubsystemBase{ - private SparkFlex flywheel = new SparkFlex(Constants.KFlywheelMotor, MotorType.kBrushless); + private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); + private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkFlexConfig leadMotor = new SparkFlexConfig(); + private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); + private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); + + // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); + public ShooterSubsystem(){ + + flywheelFollower.follow(flywheel_1, true); + backRollerFollower.follow(backRollers, true); + + flywheel_1.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + } + public void shoot(){ - flywheel.set(-0.75); - backRollers.set(-0.75); - // feedRollers.set(0.5); + flywheel_1.set(-0.75); + } public void stop(){ - flywheel.set(0); - backRollers.set(0); - + flywheel_1.set(0); + } } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java index 6d7d89a..17be09c 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java @@ -1,17 +1,50 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkFlexConfig; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; public class TransferSubsystem extends SubsystemBase{ - private SparkMax transfer = new SparkMax(14, MotorType.kBrushless); + private SparkMax transferLeadMotor = new SparkMax(Constants.kTransferMotor_1, MotorType.kBrushless); + private SparkMax transferFollowerMotor = new SparkMax(Constants.KFlywheelMotor_2, MotorType.kBrushless); + private SparkFlexConfig transferLead = new SparkFlexConfig(); + private SparkFlexConfig transferFollower = new SparkFlexConfig(); + + public TransferSubsystem(){ + + transferFollower.follow(transferLeadMotor, true); + transferLeadMotor.configure(transferLead, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + transferFollowerMotor.configure(transferFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + } + public void transfer(){ - transfer.set(1); + + transferLeadMotor.set(0.5); + + } + + public void stopTranser(){ + + transferLeadMotor.set(0); + + } + + public void outakeTransfer(){ + + transferLeadMotor.set(-0.5); + } } From bbb71114e30b276aebac4db748cb4c85cb60ba83 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 28 Feb 2026 10:51:16 -0600 Subject: [PATCH 10/19] Update ShooterSubsystem.java --- .../robot/subsystems/ShooterSubsystem.java | 121 +++++++++++++----- 1 file changed, 86 insertions(+), 35 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 660bd16..421be7f 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,49 +1,100 @@ package frc.robot.subsystems; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkFlex; +import java.util.logging.Logger; +import java.util.logging.Level; + +import frc.robot.Constants; + import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -public class ShooterSubsystem extends SubsystemBase{ +import edu.wpi.first.wpilibj2.command.SubsystemBase; - private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); - private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); - private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); - private SparkFlexConfig leadMotor = new SparkFlexConfig(); - private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); - private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); +/** + * Shoots balls currently loaded in the machine. + * @version v1.3.1 + */ +public class ShooterSubsystem extends SubsystemBase{ + private static final Logger logger = Logger.getLogger("ShooterSubsystem"); - // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); - - public ShooterSubsystem(){ - - flywheelFollower.follow(flywheel_1, true); - backRollerFollower.follow(backRollers, true); - - flywheel_1.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - } - - + private static final double MOTOR_RPM_MULT = Constants.shoooterPowerConversion; + private static final double GRAVITY = Constants.gravity_kansas; + private static final double GOAL_HEIGHT = Constants.hoopHeight; + private static final double ANGLE = Constants.shooterAngle; + private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); + private SparkMax backRollers = new SparkMax(0, MotorType.kBrushless); + private SparkMax feedRollers = new SparkMax(0, MotorType.kBrushless); + + /** + * Shoots at default power level of 50%. + * @since v0.0.0 + * @version v1.0.0 + */ public void shoot(){ - - flywheel_1.set(-0.75); - + flywheel.set(0.5); + backRollers.set(0.5); + setFeedRollers(); } - - public void stop(){ - - flywheel_1.set(0); - + + /** + * Shoots balls at manually set power level. + * @param powerLevel The power level to set the flywheels to, range 0.0 to -1.0. + * @since v1.0.0 + * @version v1.3.1 + */ + public void shoot(double powerLevel){ + // Create power level variable that we can actually change, since java no like when parameter changed + double power = powerlevel; + + if(powerLevel > 0){ + logger.info("Power level exceeds maximum output, set to -1.0"); + power = -1; + } + if(powerLevel < 0){ + logger.warning("for some reason you passed a positive value so i set the power to 0 since i dont want it to get jammed"); + power = 0; + } + + flywheel.set(powerLevel); + backRollers.set(powerLevel); + setFeedRollers(); + } + + /** + * Shoots balls at power level dependent on distance, [insert maximum shooting distance here]. + * @param distance The distance from the thing you're trying to shoot. + * @since v1.0.0 + * @version v1.3.0 + */ + public void shootAtDistance(double distance){ + + if(distance < GOAL_HEIGHT+1){ + logger.info("Too close to goal to fire!! (Minimum distance is 2.83 meters)"); + return; + } + // Finds the velocity the ball needs to travel in order to make it in the goal. (TW: Math...) + double velocity = Math.sqrt((GRAVITY*Math.pow(distance,2)) / (2 * Math.pow(Math.cos(Math.toRadians(ANGLE)),2) * (distance*Math.tan(Math.toRadians(ANGLE)) - GOAL_HEIGHT))); + // Converts velocity to target RPM (takes wheels into consideration) + double rpm = velocity*MOTOR_RPM_MULT; //motor needs to rotate X number of times so that in 1 second it spins [velocity] meters + // TODO: add a fancy log here telling you about the maximum distance if you exceed it, and then abort firing + + // Makes the motors spin at the RPM calculated + flywheel.setReference(rpm,ControlType.kVelocity); + backRollers.setReference(rpm,ControlType.kVelocity); + setFeedRollers(); } + /** + * Sets feedRollers to 0 if not firing so that it doesn't get jammed (will it get jammed otherwise?? idk, probably). + * @since v1.0.0 + * @version v1.0.0 + */ + public void setFeedRollers(){ + if(flywheel.get() < 0.05 || backRollers.get() < 0.05) + feedRollers.set(0); + else + feedRollers.set(0.5); + } } From 3744ea23dd76a00d852ae6e088d77a97c0adfd02 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 28 Feb 2026 11:48:35 -0600 Subject: [PATCH 11/19] Update Constants.java --- 2026_Rebuilt/src/main/java/frc/robot/Constants.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 482a4d7..9d7ed08 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -58,6 +58,19 @@ public final class Constants { public static final int kBackLeftSteering = 8; public static final int kBackRightSteering = 6; +<<<<<<< Updated upstream +======= + //Shooter Motor + public static final double flywheelCircumference = 0.3191858136; + public static final double backwheelCircumference = 0.0797964534; + public static final double shooterAngle = 42; //What's the meaning of life? + + //Shooter Math + public static final double hoopHeight = 1.8288; //Later, adjust this a bit so we don't keep landing rim shots (in theory) + public static final double gravity = 9.81; + public static final double gravity_kansas = 9.806; //adjusted for KANSAS SEA LEVEL BECAUSE I AM A TRYHARD + +>>>>>>> Stashed changes //Encoders public static final int kFrontLeftEncoder = 1; public static final int kFrontRightEncoder = 0; From f9ea990240b6b5639d956aa2453a2ec813b01357 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 28 Feb 2026 14:23:14 -0600 Subject: [PATCH 12/19] Moved over stuff from Yao-shooter --- .../src/main/java/frc/robot/Constants.java | 54 +------- .../robot/subsystems/ShooterSubsystem.java | 130 +++++++++++------- 2 files changed, 81 insertions(+), 103 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 9d7ed08..c6ca139 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -4,13 +4,6 @@ package frc.robot; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; /** @@ -28,22 +21,8 @@ public final class Constants { public static final int kDriverControllerPort = 0; public static final int kFunctionsControllerPort = 1; - - //Shooter Motors - public static final int KFlywheelMotor_1 = 15; - public static final int KFlywheelMotor_2 = 16; - public static final int KBackRollerMotor = 10; - - //Intake - public static final int KIntakeMotor = 11; - public static final int kPivotMotor = 12; - - //Transfer - public static final int kTransferMotor_1 = 13; - public static final int kTransferMotor_2 = 14; - - + public static final int KIntakeMotor = 10; //Drive Motors public static final int kFrontLeftDrive = 5; @@ -57,20 +36,7 @@ public final class Constants { public static final int kFrontRightSteering = 2; public static final int kBackLeftSteering = 8; public static final int kBackRightSteering = 6; - -<<<<<<< Updated upstream -======= - //Shooter Motor - public static final double flywheelCircumference = 0.3191858136; - public static final double backwheelCircumference = 0.0797964534; - public static final double shooterAngle = 42; //What's the meaning of life? - - //Shooter Math - public static final double hoopHeight = 1.8288; //Later, adjust this a bit so we don't keep landing rim shots (in theory) - public static final double gravity = 9.81; - public static final double gravity_kansas = 9.806; //adjusted for KANSAS SEA LEVEL BECAUSE I AM A TRYHARD ->>>>>>> Stashed changes //Encoders public static final int kFrontLeftEncoder = 1; public static final int kFrontRightEncoder = 0; @@ -85,7 +51,7 @@ public final class Constants { //Dampners - public static final double kSwerveDampner = 0.05; + public static final double kSwerveDampner = 0.50; public static final double kElevatorDampner = 0.5; public static final double kClimbDampner = 0.5; public static final double kAlgaeDampner = 0.2; @@ -95,20 +61,4 @@ public final class Constants { public static final double chasisWidth = Units.inchesToMeters(20); public static final double chasisLength = Units.inchesToMeters(24); - - - - //Camera - public static final AprilTagFields kField = AprilTagFields.kDefaultField; - public static final String kCameraName = "NAME"; - public static final Transform3d kRobotToCam = new Transform3d( - Units.inchesToMeters(10), 0, Units.inchesToMeters(10), - new Rotation3d(0 , 0, 0) - ); - - - //Standard Deviations - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - } diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 421be7f..f9bf5ce 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,75 +14,101 @@ /** * Shoots balls currently loaded in the machine. - * @version v1.3.1 + * @version v2.0.0 */ -public class ShooterSubsystem extends SubsystemBase{ +public class ShooterSubsystem extends SubsystemBase +{ + // logga private static final Logger logger = Logger.getLogger("ShooterSubsystem"); - - private static final double MOTOR_RPM_MULT = Constants.shoooterPowerConversion; - private static final double GRAVITY = Constants.gravity_kansas; - private static final double GOAL_HEIGHT = Constants.hoopHeight; - private static final double ANGLE = Constants.shooterAngle; - private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); + // Math-related constants + private static final double FLYWHEEL_CIRCUMFERENCE = 0.3191858136; + private static final double BACKWHEEL_CIRCUMFERENCE = 0.0797964534; + private static final double GRAVITY = 9.806; + private static final double GOAL_HEIGHT = 1.8288; + private static final double ANGLE = 42; + + // Motors + private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); //TODO: do PID tuning and get the CAN ID's private SparkMax backRollers = new SparkMax(0, MotorType.kBrushless); private SparkMax feedRollers = new SparkMax(0, MotorType.kBrushless); + + // getter land /** - * Shoots at default power level of 50%. - * @since v0.0.0 - * @version v1.0.0 - */ - public void shoot(){ - flywheel.set(0.5); - backRollers.set(0.5); - setFeedRollers(); - } - - /** - * Shoots balls at manually set power level. - * @param powerLevel The power level to set the flywheels to, range 0.0 to -1.0. - * @since v1.0.0 - * @version v1.3.1 + * Checks if the + * @return if error between target RPM and actual RPM is below 3%, return true. otherwise return false */ - public void shoot(double powerLevel){ - // Create power level variable that we can actually change, since java no like when parameter changed - double power = powerlevel; - - if(powerLevel > 0){ - logger.info("Power level exceeds maximum output, set to -1.0"); - power = -1; - } - if(powerLevel < 0){ - logger.warning("for some reason you passed a positive value so i set the power to 0 since i dont want it to get jammed"); - power = 0; - } - - flywheel.set(powerLevel); - backRollers.set(powerLevel); - setFeedRollers(); + public boolean isReadyToFire() + { + return true; //TODO: add an encoder and complete this method } - + /** - * Shoots balls at power level dependent on distance, [insert maximum shooting distance here]. + * Sets the rollers to spin to make it into a goal at the target distance. * @param distance The distance from the thing you're trying to shoot. * @since v1.0.0 - * @version v1.3.0 + * @version v2.0.0 */ - public void shootAtDistance(double distance){ + public void setDistance(double distance) + { - if(distance < GOAL_HEIGHT+1){ - logger.info("Too close to goal to fire!! (Minimum distance is 2.83 meters)"); + if(distance < GOAL_HEIGHT+0.5){ + logger.info("Too close to goal to fire!! (Minimum distance is about 2.5 meters)"); return; } // Finds the velocity the ball needs to travel in order to make it in the goal. (TW: Math...) double velocity = Math.sqrt((GRAVITY*Math.pow(distance,2)) / (2 * Math.pow(Math.cos(Math.toRadians(ANGLE)),2) * (distance*Math.tan(Math.toRadians(ANGLE)) - GOAL_HEIGHT))); - // Converts velocity to target RPM (takes wheels into consideration) - double rpm = velocity*MOTOR_RPM_MULT; //motor needs to rotate X number of times so that in 1 second it spins [velocity] meters - // TODO: add a fancy log here telling you about the maximum distance if you exceed it, and then abort firing + + rev(velocity); + } + + /** + * Revs up the flywheel and backroller to fire at a set velocity. + * @param velocity The velocity in m/s to fire the ball at. + * @since v2.0.0 + * @version v2.0.0 + */ + public void rev(double velocity) + { + // Converts velocity to target RPM + double flywheelRPM = -(velocity/FLYWHEEL_CIRCUMFERENCE)*60; + double backwheelRPM = -(velocity/BACKWHEEL_CIRCUMFERENCE)*60; // Makes the motors spin at the RPM calculated - flywheel.setReference(rpm,ControlType.kVelocity); - backRollers.setReference(rpm,ControlType.kVelocity); + flywheel.setReference(flywheelRPM,ControlType.kVelocity); + backRollers.setReference(backwheelRPM,ControlType.kVelocity); + } + + /** + * FIRE!!!! (make sure to rev up the flywheels first) + * @since v2.0.0 + * @version v2.0.0 + */ + public void shoot() + { + feedRollers.setReference(120,ControlType.kVelocity); + } + + /** + * ok now stop shooting + * @since v2.0.0 + * @version v2.0.0 + */ + public void stopShooting() + { + feedRollers.setReference(0,ControlType.kVelocity); + } + + // It's so cold here in hell, where all deprecated methods go when they die + /** + * Shoots at default power level of 50% voltage. Good for testing purposes + * @since v0.0.0 + * @version v1.3.1 + */ + public void testShot() + { + flywheel.set(-0.5); + backRollers.set(-0.5); setFeedRollers(); } @@ -90,8 +116,10 @@ public void shootAtDistance(double distance){ * Sets feedRollers to 0 if not firing so that it doesn't get jammed (will it get jammed otherwise?? idk, probably). * @since v1.0.0 * @version v1.0.0 + * @deprecated */ - public void setFeedRollers(){ + public void setFeedRollers() + { if(flywheel.get() < 0.05 || backRollers.get() < 0.05) feedRollers.set(0); else From c76b2ac05007af697a58652833068a0d8e035399 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 28 Feb 2026 14:26:03 -0600 Subject: [PATCH 13/19] Update Constants.java --- .../src/main/java/frc/robot/Constants.java | 43 +++++++++++++++++-- 1 file changed, 40 insertions(+), 3 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index c6ca139..482a4d7 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -4,6 +4,13 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; /** @@ -21,8 +28,22 @@ public final class Constants { public static final int kDriverControllerPort = 0; public static final int kFunctionsControllerPort = 1; + + //Shooter Motors + public static final int KFlywheelMotor_1 = 15; + public static final int KFlywheelMotor_2 = 16; + public static final int KBackRollerMotor = 10; + + //Intake - public static final int KIntakeMotor = 10; + public static final int KIntakeMotor = 11; + public static final int kPivotMotor = 12; + + //Transfer + public static final int kTransferMotor_1 = 13; + public static final int kTransferMotor_2 = 14; + + //Drive Motors public static final int kFrontLeftDrive = 5; @@ -36,7 +57,7 @@ public final class Constants { public static final int kFrontRightSteering = 2; public static final int kBackLeftSteering = 8; public static final int kBackRightSteering = 6; - + //Encoders public static final int kFrontLeftEncoder = 1; public static final int kFrontRightEncoder = 0; @@ -51,7 +72,7 @@ public final class Constants { //Dampners - public static final double kSwerveDampner = 0.50; + public static final double kSwerveDampner = 0.05; public static final double kElevatorDampner = 0.5; public static final double kClimbDampner = 0.5; public static final double kAlgaeDampner = 0.2; @@ -61,4 +82,20 @@ public final class Constants { public static final double chasisWidth = Units.inchesToMeters(20); public static final double chasisLength = Units.inchesToMeters(24); + + + + //Camera + public static final AprilTagFields kField = AprilTagFields.kDefaultField; + public static final String kCameraName = "NAME"; + public static final Transform3d kRobotToCam = new Transform3d( + Units.inchesToMeters(10), 0, Units.inchesToMeters(10), + new Rotation3d(0 , 0, 0) + ); + + + //Standard Deviations + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } From 3cc32a94609c8d75571f24fcb5700cd2f7c3b8db Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 7 Mar 2026 10:50:15 -0600 Subject: [PATCH 14/19] Update ShooterSubsystem.java --- .../robot/subsystems/ShooterSubsystem.java | 39 ++++++++++++++----- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f9bf5ce..4333924 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,12 +27,28 @@ public class ShooterSubsystem extends SubsystemBase private static final double GOAL_HEIGHT = 1.8288; private static final double ANGLE = 42; - // Motors - private SparkMax flywheel = new SparkMax(0, MotorType.kBrushless); //TODO: do PID tuning and get the CAN ID's - private SparkMax backRollers = new SparkMax(0, MotorType.kBrushless); - private SparkMax feedRollers = new SparkMax(0, MotorType.kBrushless); + // The flywheel runs on two separate motors. + private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); + private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); + + private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkFlexConfig leadMotor = new SparkFlexConfig(); + private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); + private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); + + // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); + public ShooterSubsystem(){ + + flywheelFollower.follow(flywheel_1, true); + backRollerFollower.follow(backRollers, true); + + flywheel_1.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + } // getter land /** * Checks if the @@ -40,7 +56,7 @@ public class ShooterSubsystem extends SubsystemBase */ public boolean isReadyToFire() { - return true; //TODO: add an encoder and complete this method + return true; //TODO: get the encoders and complete this method } /** @@ -71,12 +87,15 @@ public void setDistance(double distance) public void rev(double velocity) { // Converts velocity to target RPM - double flywheelRPM = -(velocity/FLYWHEEL_CIRCUMFERENCE)*60; - double backwheelRPM = -(velocity/BACKWHEEL_CIRCUMFERENCE)*60; + double flywheelRPM = (velocity/FLYWHEEL_CIRCUMFERENCE)*60; + double backRollerRPM = (velocity/BACKWHEEL_CIRCUMFERENCE)*60; // if you need to reverse it do it here - // Makes the motors spin at the RPM calculated - flywheel.setReference(flywheelRPM,ControlType.kVelocity); - backRollers.setReference(backwheelRPM,ControlType.kVelocity); + // Makes the flywheel motors spin at the RPM calculated + flywheel_1.setReference(flywheelRPM,ControlType.kVelocity); + flywheel_2.setReference(-flywheelRPM,ControlType.kVelocity);// in reverse + + // Then the backrollers + backRollers.setReference(backRollerRPM,ControlType.kVelocity); } /** From 1e1200785fc265a9a422ae16b72aa0d09f1db197 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 7 Mar 2026 11:46:27 -0600 Subject: [PATCH 15/19] Added some comments. --- .../src/main/java/frc/robot/Constants.java | 15 +++------ .../robot/subsystems/ShooterSubsystem.java | 31 ++++++++++++------- 2 files changed, 25 insertions(+), 21 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index 482a4d7..4ca1111 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -23,7 +23,6 @@ **/ public final class Constants { - //Controllers public static final int kDriverControllerPort = 0; public static final int kFunctionsControllerPort = 1; @@ -33,17 +32,17 @@ public final class Constants { public static final int KFlywheelMotor_1 = 15; public static final int KFlywheelMotor_2 = 16; public static final int KBackRollerMotor = 10; - + //Intake public static final int KIntakeMotor = 11; public static final int kPivotMotor = 12; + //Transfer public static final int kTransferMotor_1 = 13; public static final int kTransferMotor_2 = 14; - //Drive Motors public static final int kFrontLeftDrive = 5; @@ -51,14 +50,13 @@ public final class Constants { public static final int kBackLeftDrive = 9; public static final int kBackRightDrive = 7; - //Steering Motors public static final int kFrontLeftSteering = 4; public static final int kFrontRightSteering = 2; public static final int kBackLeftSteering = 8; public static final int kBackRightSteering = 6; - //Encoders + //Drivetrain Encoders public static final int kFrontLeftEncoder = 1; public static final int kFrontRightEncoder = 0; public static final int kBackLeftEncoder = 2; @@ -70,21 +68,18 @@ public final class Constants { public static final double kBackLeftEncoderOffset = 0.25; public static final double kBackRightEncoderOffset = 0.07; - - //Dampners + //Dampeners public static final double kSwerveDampner = 0.05; public static final double kElevatorDampner = 0.5; public static final double kClimbDampner = 0.5; public static final double kAlgaeDampner = 0.2; - + //Chasis Length public static final double chasisWidth = Units.inchesToMeters(20); public static final double chasisLength = Units.inchesToMeters(24); - - //Camera public static final AprilTagFields kField = AprilTagFields.kDefaultField; public static final String kCameraName = "NAME"; diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 4333924..5286c2c 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,7 +14,7 @@ /** * Shoots balls currently loaded in the machine. - * @version v2.0.0 + * @version v2.1.0 */ public class ShooterSubsystem extends SubsystemBase { @@ -23,15 +23,16 @@ public class ShooterSubsystem extends SubsystemBase // Math-related constants private static final double FLYWHEEL_CIRCUMFERENCE = 0.3191858136; private static final double BACKWHEEL_CIRCUMFERENCE = 0.0797964534; - private static final double GRAVITY = 9.806; - private static final double GOAL_HEIGHT = 1.8288; - private static final double ANGLE = 42; + private static final double GRAVITY = 9.806; //adjusted for kansas sea level + private static final double GOAL_HEIGHT = 1.8288; //difference between shooter height and goal height in meters + private static final double ANGLE = 42; //what's the meaning of life? // The flywheel runs on two separate motors. private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkFlexConfig leadMotor = new SparkFlexConfig(); private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); @@ -39,7 +40,8 @@ public class ShooterSubsystem extends SubsystemBase // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); - public ShooterSubsystem(){ + public ShooterSubsystem() + { flywheelFollower.follow(flywheel_1, true); backRollerFollower.follow(backRollers, true); @@ -49,10 +51,15 @@ public ShooterSubsystem(){ backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } - // getter land + + public void periodic() + { + + } + /** - * Checks if the - * @return if error between target RPM and actual RPM is below 3%, return true. otherwise return false + * Checks if the actual RPM of the flywheel/backroller is close to the target RPM + * @return boolean calculated upon running this method */ public boolean isReadyToFire() { @@ -82,7 +89,7 @@ public void setDistance(double distance) * Revs up the flywheel and backroller to fire at a set velocity. * @param velocity The velocity in m/s to fire the ball at. * @since v2.0.0 - * @version v2.0.0 + * @version v2.1.0 */ public void rev(double velocity) { @@ -102,20 +109,22 @@ public void rev(double velocity) * FIRE!!!! (make sure to rev up the flywheels first) * @since v2.0.0 * @version v2.0.0 + * @deprecated fix later */ public void shoot() { - feedRollers.setReference(120,ControlType.kVelocity); + // feedRollers.setReference(120,ControlType.kVelocity); } /** * ok now stop shooting * @since v2.0.0 * @version v2.0.0 + * @deprecated fix later */ public void stopShooting() { - feedRollers.setReference(0,ControlType.kVelocity); + // feedRollers.setReference(0,ControlType.kVelocity); } // It's so cold here in hell, where all deprecated methods go when they die From 23454d831ce2a931a286cd8e8790d4a822d8a87e Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 7 Mar 2026 12:09:39 -0600 Subject: [PATCH 16/19] Completed isReadyToFire() --- .../robot/subsystems/ShooterSubsystem.java | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5286c2c..237bbc0 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -26,13 +26,17 @@ public class ShooterSubsystem extends SubsystemBase private static final double GRAVITY = 9.806; //adjusted for kansas sea level private static final double GOAL_HEIGHT = 1.8288; //difference between shooter height and goal height in meters private static final double ANGLE = 42; //what's the meaning of life? + private static final double RPM_TOLERANCE = 0.05; //5% error allowed // The flywheel runs on two separate motors. private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); - - private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private double flywheelTargetRPM; + // The backroller runs on only one motor. + private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private double backRollerTargetRPM; + //Follower motors. private SparkFlexConfig leadMotor = new SparkFlexConfig(); private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); @@ -50,6 +54,7 @@ public ShooterSubsystem() flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } public void periodic() @@ -63,7 +68,18 @@ public void periodic() */ public boolean isReadyToFire() { - return true; //TODO: get the encoders and complete this method + double flywheelActualRPM = flywheel_1.getEncoder().getVelocity(); + double backRollerActualRPM = backRollers.getEncoder().getVelocity(); + + // Checks if flywheel target RPM is within tolerance + if(Math.abs((flywheelActualRPM-flywheelTargetRPM)/flywheelTargetRPM) > RPM_TOLERANCE) + return false; + // Checks if backroller target RPM is within tolerance + if(Math.abs((backRollerActualRPM-backRollerTargetRPM)/backRollerTargetRPM) > RPM_TOLERANCE) + return false; + + // if both checks succeed, return true + return true; } /** @@ -94,15 +110,15 @@ public void setDistance(double distance) public void rev(double velocity) { // Converts velocity to target RPM - double flywheelRPM = (velocity/FLYWHEEL_CIRCUMFERENCE)*60; - double backRollerRPM = (velocity/BACKWHEEL_CIRCUMFERENCE)*60; // if you need to reverse it do it here + flywheelTargetRPM = (velocity/FLYWHEEL_CIRCUMFERENCE)*60; + backRollerTargetRPM = (velocity/BACKWHEEL_CIRCUMFERENCE)*60; // if you need to reverse it do it here // Makes the flywheel motors spin at the RPM calculated - flywheel_1.setReference(flywheelRPM,ControlType.kVelocity); - flywheel_2.setReference(-flywheelRPM,ControlType.kVelocity);// in reverse + flywheel_1.setReference(flywheelTargetRPM,ControlType.kVelocity); + flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity);// in reverse // Then the backrollers - backRollers.setReference(backRollerRPM,ControlType.kVelocity); + backRollers.setReference(backRollerTargetRPM,ControlType.kVelocity); } /** From 0c94586ab568e5acff58bddd769d4f9312b519c4 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Sat, 7 Mar 2026 14:14:59 -0600 Subject: [PATCH 17/19] Update ShooterSubsystem.java --- .../robot/subsystems/ShooterSubsystem.java | 120 +++++++----------- 1 file changed, 44 insertions(+), 76 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 237bbc0..47e6208 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -13,20 +13,23 @@ /** - * Shoots balls currently loaded in the machine. - * @version v2.1.0 + * This subsystem calculates and sets the shooter's power. + * @version v2.2.0 */ public class ShooterSubsystem extends SubsystemBase { - // logga - private static final Logger logger = Logger.getLogger("ShooterSubsystem"); - // Math-related constants + // Measurement constants (in meters) private static final double FLYWHEEL_CIRCUMFERENCE = 0.3191858136; private static final double BACKWHEEL_CIRCUMFERENCE = 0.0797964534; + private static final double GOAL_HEIGHT = 1.397; //difference between shooter height and goal height in meters + private static final double MINIMUM_FIRING_DISTANCE = 2.5; // This is pretty arbritary but it makes sure that setDistance() doesn't just try to shoot straight at the hoop + private static final double MAXIMUM_FIRING_DISTANCE = 5.0; // Note: Can shoot much further, albeit not as accurately. Rough estimate based off backroller's max RPM + // Other math-related constants private static final double GRAVITY = 9.806; //adjusted for kansas sea level - private static final double GOAL_HEIGHT = 1.8288; //difference between shooter height and goal height in meters private static final double ANGLE = 42; //what's the meaning of life? private static final double RPM_TOLERANCE = 0.05; //5% error allowed + private static final double MOTOR_MAX_RPM = 6784; + private static final double POWER_MULT = 1/0.9; //Approx reciprocal of the % of velocity transfered to the ball from the flywheel (efficiency) // The flywheel runs on two separate motors. private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); @@ -36,68 +39,61 @@ public class ShooterSubsystem extends SubsystemBase // The backroller runs on only one motor. private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); private double backRollerTargetRPM; + //Follower motors. private SparkFlexConfig leadMotor = new SparkFlexConfig(); private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); - - // private SparkMax feedRollers = new SparkMax(13, MotorType.kBrushless); - public ShooterSubsystem() { - flywheelFollower.follow(flywheel_1, true); backRollerFollower.follow(backRollers, true); flywheel_1.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - - } - - public void periodic() - { - } /** - * Checks if the actual RPM of the flywheel/backroller is close to the target RPM + * Checks if the actual RPM of the flywheel is close to the target RPM. Backroller is negligible * @return boolean calculated upon running this method + * @since v2.0.0 + * @version v2.2.0 */ public boolean isReadyToFire() { + // Gets current velocity from encoder double flywheelActualRPM = flywheel_1.getEncoder().getVelocity(); - double backRollerActualRPM = backRollers.getEncoder().getVelocity(); // Checks if flywheel target RPM is within tolerance if(Math.abs((flywheelActualRPM-flywheelTargetRPM)/flywheelTargetRPM) > RPM_TOLERANCE) return false; - // Checks if backroller target RPM is within tolerance - if(Math.abs((backRollerActualRPM-backRollerTargetRPM)/backRollerTargetRPM) > RPM_TOLERANCE) - return false; - - // if both checks succeed, return true return true; } /** - * Sets the rollers to spin to make it into a goal at the target distance. - * @param distance The distance from the thing you're trying to shoot. + * Calculates velocity needed to make a goal from distance, then runs rev() method + * @param distance The distance from the thing you're trying to shoot. (Minimum distance: 2 meters. Maximum (accurate) distance: 6 meters) * @since v1.0.0 - * @version v2.0.0 + * @version v2.2.0 */ public void setDistance(double distance) { - - if(distance < GOAL_HEIGHT+0.5){ - logger.info("Too close to goal to fire!! (Minimum distance is about 2.5 meters)"); + // Checks if robot is too close to fire + if(distance < MINIMUM_FIRING_DISTANCE) + { + System.out.println("Too close to goal to fire!! (Minimum distance is 2.5 meters)"); return; } + // Checks if robot is too far to fire accurately + if(distance < MINIMUM_FIRING_DISTANCE) + System.out.println("Distance exceeds 5 meters, launcher will likely undershoot (not a big problem unless you're trying to make goals)"); + // Finds the velocity the ball needs to travel in order to make it in the goal. (TW: Math...) double velocity = Math.sqrt((GRAVITY*Math.pow(distance,2)) / (2 * Math.pow(Math.cos(Math.toRadians(ANGLE)),2) * (distance*Math.tan(Math.toRadians(ANGLE)) - GOAL_HEIGHT))); + // Sets motors to fire at calculated velocity rev(velocity); } @@ -110,63 +106,35 @@ public void setDistance(double distance) public void rev(double velocity) { // Converts velocity to target RPM - flywheelTargetRPM = (velocity/FLYWHEEL_CIRCUMFERENCE)*60; - backRollerTargetRPM = (velocity/BACKWHEEL_CIRCUMFERENCE)*60; // if you need to reverse it do it here + flywheelTargetRPM = Math.min((velocity/FLYWHEEL_CIRCUMFERENCE)*60*POWER_MULT,MOTOR_MAX_RPM); + backRollerTargetRPM = Math.min((velocity/BACKWHEEL_CIRCUMFERENCE)*60,MOTOR_MAX_RPM); // Note to self: if you need to reverse it do it here // Makes the flywheel motors spin at the RPM calculated flywheel_1.setReference(flywheelTargetRPM,ControlType.kVelocity); - flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity);// in reverse + flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse - // Then the backrollers + // Then the backrollers (These will almost always have to fire near 100% velocity. Why??? Who designed this thing??) + // It's too late to put a 3:2 gear ratio on it :cry: backRollers.setReference(backRollerTargetRPM,ControlType.kVelocity); } /** - * FIRE!!!! (make sure to rev up the flywheels first) - * @since v2.0.0 - * @version v2.0.0 - * @deprecated fix later + * Warms up the flywheel and backroller so that it takes less time for the flywheel to hit target speed. + * @since v2.2.0 + * @version 2.2.0 */ - public void shoot() + public void preRev(double velocity) { - // feedRollers.setReference(120,ControlType.kVelocity); - } - - /** - * ok now stop shooting - * @since v2.0.0 - * @version v2.0.0 - * @deprecated fix later - */ - public void stopShooting() - { - // feedRollers.setReference(0,ControlType.kVelocity); - } + // Sets target RPMs + flywheelTargetRPM = 1500; + backRollerTargetRPM = 6000; + + // Makes the flywheel motors spin + flywheel_1.setReference(flywheelTargetRPM,ControlType.kVelocity); + flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse - // It's so cold here in hell, where all deprecated methods go when they die - /** - * Shoots at default power level of 50% voltage. Good for testing purposes - * @since v0.0.0 - * @version v1.3.1 - */ - public void testShot() - { - flywheel.set(-0.5); - backRollers.set(-0.5); - setFeedRollers(); + // Then the backrollers + backRollers.setReference(backRollerTargetRPM,ControlType.kVelocity); } - /** - * Sets feedRollers to 0 if not firing so that it doesn't get jammed (will it get jammed otherwise?? idk, probably). - * @since v1.0.0 - * @version v1.0.0 - * @deprecated - */ - public void setFeedRollers() - { - if(flywheel.get() < 0.05 || backRollers.get() < 0.05) - feedRollers.set(0); - else - feedRollers.set(0.5); - } } From f84b8b9675f942ce0bc96461547c2fb0c798787c Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Mon, 9 Mar 2026 18:28:56 -0500 Subject: [PATCH 18/19] added a single comment --- .../src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 47e6208..f7df35b 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -21,7 +21,7 @@ public class ShooterSubsystem extends SubsystemBase // Measurement constants (in meters) private static final double FLYWHEEL_CIRCUMFERENCE = 0.3191858136; private static final double BACKWHEEL_CIRCUMFERENCE = 0.0797964534; - private static final double GOAL_HEIGHT = 1.397; //difference between shooter height and goal height in meters + private static final double GOAL_HEIGHT = 1.397; //difference between shooter height and goal height in meters private static final double MINIMUM_FIRING_DISTANCE = 2.5; // This is pretty arbritary but it makes sure that setDistance() doesn't just try to shoot straight at the hoop private static final double MAXIMUM_FIRING_DISTANCE = 5.0; // Note: Can shoot much further, albeit not as accurately. Rough estimate based off backroller's max RPM // Other math-related constants @@ -30,6 +30,7 @@ public class ShooterSubsystem extends SubsystemBase private static final double RPM_TOLERANCE = 0.05; //5% error allowed private static final double MOTOR_MAX_RPM = 6784; private static final double POWER_MULT = 1/0.9; //Approx reciprocal of the % of velocity transfered to the ball from the flywheel (efficiency) + // ^ This doesn't apply to the backrollers, which is weird and implicit but provides backspin to the ball // The flywheel runs on two separate motors. private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); From 3b3cadc8f1e5db900ed9a01e98c6999bcad90ea3 Mon Sep 17 00:00:00 2001 From: Yao Pan Date: Mon, 9 Mar 2026 19:50:22 -0500 Subject: [PATCH 19/19] apparently sparkflex uses closed loop controller objects and i had to deal with those --- .../main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 39 ++++++++++++------- 2026_Rebuilt/vendordeps/REVLib.json | 18 ++++----- 2026_Rebuilt/vendordeps/photonlib.json | 12 +++--- 4 files changed, 40 insertions(+), 31 deletions(-) diff --git a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java index 907d03b..e46104e 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -38,7 +38,7 @@ public class RobotContainer { private final TransferSubsystem transferSubsystem = new TransferSubsystem(); private final VisionSubsystem visionSubsystem = new VisionSubsystem(Constants.kCameraName, Constants.kField, Constants.kRobotToCam, swerveDrivetrainSubsystem::addVisionMeasurements); - private final RunCommand shoot = new RunCommand(()-> this.shooterSubsystem.shoot(), shooterSubsystem); + private final RunCommand shoot = new RunCommand(()-> this.shooterSubsystem.rev(), shooterSubsystem); private final RunCommand intake = new RunCommand(()-> this.intakeSubsystem.intake(), intakeSubsystem); private final RunCommand outake = new RunCommand(()-> this.intakeSubsystem.outake(), intakeSubsystem); private final RunCommand transfer = new RunCommand(()-> this.transferSubsystem.transfer(), transferSubsystem); diff --git a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f7df35b..ff48b70 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -6,6 +6,12 @@ import frc.robot.Constants; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -33,12 +39,15 @@ public class ShooterSubsystem extends SubsystemBase // ^ This doesn't apply to the backrollers, which is weird and implicit but provides backspin to the ball // The flywheel runs on two separate motors. - private SparkFlex flywheel_1 = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); - private SparkFlex flywheel_2 = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); + private SparkFlex flywheel_1_motor = new SparkFlex(Constants.KFlywheelMotor_1, MotorType.kBrushless); + private SparkClosedLoopController flywheel_1 = flywheel_1_motor.getClosedLoopController(); + private SparkFlex flywheel_2_motor = new SparkFlex(Constants.KFlywheelMotor_2, MotorType.kBrushless); + private SparkClosedLoopController flywheel_2 = flywheel_2_motor.getClosedLoopController(); private double flywheelTargetRPM; // The backroller runs on only one motor. - private SparkFlex backRollers = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkFlex backRollers_motor = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkClosedLoopController backRollers = backRollers_motor.getClosedLoopController(); private double backRollerTargetRPM; //Follower motors. @@ -48,12 +57,12 @@ public class ShooterSubsystem extends SubsystemBase public ShooterSubsystem() { - flywheelFollower.follow(flywheel_1, true); - backRollerFollower.follow(backRollers, true); + flywheelFollower.follow(flywheel_1_motor, true); + backRollerFollower.follow(backRollers_motor, true); - flywheel_1.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - flywheel_2.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - backRollers.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + flywheel_1_motor.configure(leadMotor, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + flywheel_2_motor.configure(flywheelFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + backRollers_motor.configure(backRollerFollower, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } /** @@ -65,7 +74,7 @@ public ShooterSubsystem() public boolean isReadyToFire() { // Gets current velocity from encoder - double flywheelActualRPM = flywheel_1.getEncoder().getVelocity(); + double flywheelActualRPM = flywheel_1_motor.getEncoder().getVelocity(); // Checks if flywheel target RPM is within tolerance if(Math.abs((flywheelActualRPM-flywheelTargetRPM)/flywheelTargetRPM) > RPM_TOLERANCE) @@ -111,12 +120,12 @@ public void rev(double velocity) backRollerTargetRPM = Math.min((velocity/BACKWHEEL_CIRCUMFERENCE)*60,MOTOR_MAX_RPM); // Note to self: if you need to reverse it do it here // Makes the flywheel motors spin at the RPM calculated - flywheel_1.setReference(flywheelTargetRPM,ControlType.kVelocity); - flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse + flywheel_1.setSetpoint(flywheelTargetRPM,ControlType.kVelocity); + flywheel_2.setSetpoint(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse // Then the backrollers (These will almost always have to fire near 100% velocity. Why??? Who designed this thing??) // It's too late to put a 3:2 gear ratio on it :cry: - backRollers.setReference(backRollerTargetRPM,ControlType.kVelocity); + backRollers.setSetpoint(backRollerTargetRPM,ControlType.kVelocity); } /** @@ -131,11 +140,11 @@ public void preRev(double velocity) backRollerTargetRPM = 6000; // Makes the flywheel motors spin - flywheel_1.setReference(flywheelTargetRPM,ControlType.kVelocity); - flywheel_2.setReference(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse + flywheel_1.setSetpoint(flywheelTargetRPM,ControlType.kVelocity); + flywheel_2.setSetpoint(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse // Then the backrollers - backRollers.setReference(backRollerTargetRPM,ControlType.kVelocity); + backRollers.setSetpoint(backRollerTargetRPM,ControlType.kVelocity); } } diff --git a/2026_Rebuilt/vendordeps/REVLib.json b/2026_Rebuilt/vendordeps/REVLib.json index a5af3e9..e8196c3 100644 --- a/2026_Rebuilt/vendordeps/REVLib.json +++ b/2026_Rebuilt/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.3", + "version": "2026.0.4", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.3" + "version": "2026.0.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.3", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.3", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.3", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.3", + "version": "2026.0.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.3", + "version": "2026.0.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.3", + "version": "2026.0.4", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.3", + "version": "2026.0.4", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/2026_Rebuilt/vendordeps/photonlib.json b/2026_Rebuilt/vendordeps/photonlib.json index fbb5c80..62f135c 100644 --- a/2026_Rebuilt/vendordeps/photonlib.json +++ b/2026_Rebuilt/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.2.2", + "version": "v2026.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.2.2", + "version": "v2026.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.2.2", + "version": "v2026.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.2.2", + "version": "v2026.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.2.2" + "version": "v2026.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.2.2" + "version": "v2026.3.1" } ] } \ No newline at end of file