diff --git a/2026_Rebuilt/src/main/java/frc/robot/Constants.java b/2026_Rebuilt/src/main/java/frc/robot/Constants.java index c50ba05..4ca1111 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/Constants.java +++ b/2026_Rebuilt/src/main/java/frc/robot/Constants.java @@ -4,6 +4,15 @@ 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; + /** * 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,9 +20,77 @@ * *

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; - } + 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; + + + //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; + + //Drivetrain 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; + + //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"; + 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/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..e46104e 100644 --- a/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java +++ b/2026_Rebuilt/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,21 @@ 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.IntakeSubsystem; +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; +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 +29,40 @@ * 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(); + public 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 VisionSubsystem visionSubsystem = new VisionSubsystem(Constants.kCameraName, Constants.kField, Constants.kRobotToCam, swerveDrivetrainSubsystem::addVisionMeasurements); + + 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); + 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 CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + public static 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(); + + visionSubsystem.periodic(); + + } /** @@ -42,13 +75,20 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + m_driverController.button(6).onTrue(shoot); + m_driverController.button(2).onTrue(stop); + m_driverController.button(4).onTrue(outake); + m_driverController.button(5).onTrue(intake); + m_driverController.button(1).onTrue(stopIntake); + // 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 +96,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..c595def --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java @@ -0,0 +1,151 @@ +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)); + } + + public SwerveModuleState getState() { + return this.getState(); + } + +} + 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..32f16c4 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,74 @@ +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(){ + intakeRoller.set(0.5); + } + + public void outake(){ + intakeRoller.set(-0.5); + } + + 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 new file mode 100644 index 0000000..ff48b70 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,150 @@ +package frc.robot.subsystems; + +import java.util.logging.Logger; +import java.util.logging.Level; + +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; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +/** + * This subsystem calculates and sets the shooter's power. + * @version v2.2.0 + */ +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 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 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) + // ^ 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_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_motor = new SparkFlex(Constants.KBackRollerMotor, MotorType.kBrushless); + private SparkClosedLoopController backRollers = backRollers_motor.getClosedLoopController(); + private double backRollerTargetRPM; + + //Follower motors. + private SparkFlexConfig leadMotor = new SparkFlexConfig(); + private SparkFlexConfig flywheelFollower = new SparkFlexConfig(); + private SparkFlexConfig backRollerFollower = new SparkFlexConfig(); + + public ShooterSubsystem() + { + flywheelFollower.follow(flywheel_1_motor, true); + backRollerFollower.follow(backRollers_motor, true); + + 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); + } + + /** + * 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_motor.getEncoder().getVelocity(); + + // Checks if flywheel target RPM is within tolerance + if(Math.abs((flywheelActualRPM-flywheelTargetRPM)/flywheelTargetRPM) > RPM_TOLERANCE) + return false; + return true; + } + + /** + * 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.2.0 + */ + public void setDistance(double distance) + { + // 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); + } + + /** + * 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.1.0 + */ + public void rev(double velocity) + { + // Converts velocity to target RPM + 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.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.setSetpoint(backRollerTargetRPM,ControlType.kVelocity); + } + + /** + * 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 preRev(double velocity) + { + // Sets target RPMs + flywheelTargetRPM = 1500; + backRollerTargetRPM = 6000; + + // Makes the flywheel motors spin + flywheel_1.setSetpoint(flywheelTargetRPM,ControlType.kVelocity); + flywheel_2.setSetpoint(-flywheelTargetRPM,ControlType.kVelocity); // This one's in reverse + + // Then the backrollers + backRollers.setSetpoint(backRollerTargetRPM,ControlType.kVelocity); + } + +} 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..3496475 --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/SwerveDrivetrainSubsystem.java @@ -0,0 +1,210 @@ +// 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.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; +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.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; +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()}; + SwerveDrivePoseEstimator drivePoseEstimator; + + + 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(){ + + 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); + + + } + + // 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 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/TransferSubsystem.java b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java new file mode 100644 index 0000000..17be09c --- /dev/null +++ b/2026_Rebuilt/src/main/java/frc/robot/subsystems/TransferSubsystem.java @@ -0,0 +1,51 @@ +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 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(){ + + transferLeadMotor.set(0.5); + + } + + public void stopTranser(){ + + transferLeadMotor.set(0); + + } + + public void outakeTransfer(){ + + transferLeadMotor.set(-0.5); + + } + +} + \ 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 new file mode 100644 index 0000000..e8196c3 --- /dev/null +++ b/2026_Rebuilt/vendordeps/REVLib.json @@ -0,0 +1,133 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.4", + "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.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.4", + "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.4", + "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.4", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.4", + "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 diff --git a/2026_Rebuilt/vendordeps/photonlib.json b/2026_Rebuilt/vendordeps/photonlib.json new file mode 100644 index 0000000..62f135c --- /dev/null +++ b/2026_Rebuilt/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.3.1", + "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.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.3.1" + } + ] +} \ No newline at end of file