From 56677170e5bf690dae0cd68ac643d5251e932757 Mon Sep 17 00:00:00 2001 From: Johnjacob12345 <162513125+Johnjacob12345@users.noreply.github.com> Date: Fri, 6 Mar 2026 20:08:18 -0800 Subject: [PATCH 01/10] Auto --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 22 +++-- src/main/java/frc/robot/commands/Autos.java | 7 +- .../robot/subsystems/AgitatorSubsystem.java | 3 + .../frc/robot/subsystems/DriveSubsystem.java | 84 ++++++++++++------- .../frc/robot/subsystems/IntakeSubsystem.java | 7 ++ .../frc/robot/subsystems/PhotonVision.java | 18 ++-- .../robot/subsystems/ShooterSubsystem.java | 30 +++---- 9 files changed, 118 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f32d291..f59af39 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,7 +80,7 @@ public static final class Intake { public static final class Vision { //Target public static final double kYawTarget = -8; //Degrees - public static final double kDistanceTarget = 1.8; //Meters + public static final double kDistanceTarget = 1.9; //Meters public static final AprilTagFieldLayout kAprilTagFieldLayout = AprilTagFieldLayout .loadField(AprilTagFields.kDefaultField); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8fa786f..d572ecf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj.DriverStation; /** @@ -19,6 +20,7 @@ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; + private Command m_driveForwardCommand; private final RobotContainer m_robotContainer; // Added fields @@ -82,16 +84,21 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_driveForwardCommand = m_robotContainer.DriveForwardCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + CommandScheduler.getInstance().schedule(Commands.sequence( + m_driveForwardCommand, + m_autonomousCommand) + ); } } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24b7952..f9b600e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,6 +44,7 @@ public class RobotContainer { private final AgitatorSubsystem m_AgitatorSubsystem = new AgitatorSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(Driver.kJoystickID); @@ -57,6 +58,7 @@ public class RobotContainer { */ public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser(); // Configure the trigger bindings @@ -107,9 +109,8 @@ private double applyDeadbandAndScale(double value) { // Register named commands for use in PathPlanner autonomous paths private void registerNamedCommands() { - NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.shoot_1_Command()); NamedCommands.registerCommand("Shooter_ON", m_ShooterSubsystem.StartShoot()); - NamedCommands.registerCommand("Shooter_OFF", m_ShooterSubsystem.StopShoot()); + NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.StartShoot()); NamedCommands.registerCommand("Climb_UP", m_ClimberSubsystem.UpClimb()); NamedCommands.registerCommand("Climb_DOWN", m_ClimberSubsystem.DownClimb()); NamedCommands.registerCommand("Intake_ON", m_IntakeSubsystem.StartIntake()); @@ -122,12 +123,17 @@ private void configureBindings() { // Driver Controls // Shooter control m_driverController.rightBumper() - .whileTrue(m_photonVision.AimShoot()); + .onTrue(m_photonVision.AimShoot()); + // Start Shooter (constant speed) m_driverController.rightTrigger() - .whileTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake())); - // .onFalse(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake())); + .onTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake())); + + + +m_driverController.rightBumper().negate().and(m_driverController.rightTrigger().negate()) + .onTrue(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake())); // Climber control m_operatorController.a() @@ -172,6 +178,10 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous - return autoChooser.getSelected(); + return m_photonVision.AimShoot(); + } + public Command DriveForwardCommand() { + // An example command will be run in autonomous + return m_driveSubsystem.DriveForward(); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index ae5faeb..2d6d128 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,14 +4,15 @@ package frc.robot.commands; -import frc.robot.subsystems.AgitatorSubsystem; +import frc.robot.subsystems.PhotonVision; +import frc.robot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public final class Autos { /** Example static factory for an autonomous command. */ - public static Command exampleAuto(AgitatorSubsystem subsystem) { - return Commands.sequence(subsystem.StartAgitator(), new ExampleCommand(subsystem)); + public static Command exampleAuto(PhotonVision vision, DriveSubsystem drive) { + return Commands.sequence(vision.AimShoot()); } private Autos() { diff --git a/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java b/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java index c3f89b1..9fa4fc6 100644 --- a/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java @@ -61,6 +61,9 @@ public Command StartAgitator() { } + public void StartAgitatorVoid() { + AgitatorMotor.set(-Constants.Subsystems.Agitator.kMaxAgitatorSpeed); } + public Command StopAgitator() { return this.run(() -> { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e6a44e8..d218b6f 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -3,13 +3,13 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; - // For CAN import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -24,17 +24,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; +import frc.robot.subsystems.PhotonVision; import java.util.Optional; import com.ctre.phoenix6.hardware.Pigeon2; //Gyro public class DriveSubsystem extends SubsystemBase { - - private final Field2d field = new Field2d(); + private final Field2d field = new Field2d(); private final Pigeon2 pigeon = new Pigeon2(Constants.Subsystems.Drive.kGyroPort); @@ -50,8 +50,8 @@ public class DriveSubsystem extends SubsystemBase { private final RelativeEncoder rightEncoder = rightMaster.getEncoder(); private final RelativeEncoder leftEncoder = leftMaster.getEncoder(); - - + boolean back = false; + boolean done = false; double leftDis = 0; double leftPos = 0; double rightPos = 0; @@ -65,27 +65,21 @@ public class DriveSubsystem extends SubsystemBase { private Pose2d currentPose = new Pose2d(); - private final DifferentialDrivePoseEstimator m_poseEstimator = - new DifferentialDrivePoseEstimator( - Constants.Subsystems.Drive.kinematics, - pigeon.getRotation2d(), - leftEncoder.getPosition() / 8.45 * wheelCircumference, - rightEncoder.getPosition() / 8.45 * wheelCircumference, - new Pose2d(), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), - VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); - - + private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator( + Constants.Subsystems.Drive.kinematics, + pigeon.getRotation2d(), + leftEncoder.getPosition() / 8.45 * wheelCircumference, + rightEncoder.getPosition() / 8.45 * wheelCircumference, + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); public DriveSubsystem() { - SmartDashboard.putData("Field", field); - leftMaster.setInverted(true); - leftFollower.setInverted(true); - - - + SmartDashboard.putData("Field", field); + leftMaster.setInverted(true); + leftFollower.setInverted(true); pigeon.setYaw(0.0); @@ -127,6 +121,33 @@ public void arcadeDrive(double fwd, double rot) { // } } + + + public Command DriveForward() { + leftDis = 0; + rightDis = 0; + done = false; + + return new RunCommand(() -> { + if (leftDis < 0.3 && !back) { + drive.arcadeDrive(-1, 0); + SmartDashboard.putNumber("Dis", leftDis); + } else if (leftDis > -0.3) { + back = true; + drive.arcadeDrive(1, 0); + + } else { + + drive.arcadeDrive(0, 0); + + + + } + + }, this).until(() -> done); + + } + public void tankDrive(double left, double right) { drive.tankDrive(left, right); } @@ -162,7 +183,7 @@ public void driveRobotRelative(ChassisSpeeds speeds) { speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); } - + public Command resetPigeon() { return this.run(() -> { @@ -172,9 +193,14 @@ public Command resetPigeon() { } + // public Command driveForward(){ + // if () + // // drive.arcadeDrive(-0.4, 0); + + // } public Pose2d get2dPose() { - Pose2d pose = m_poseEstimator.getEstimatedPosition(); + Pose2d pose = m_poseEstimator.getEstimatedPosition(); return pose; } @@ -185,14 +211,14 @@ public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) { @Override public void periodic() { // Put periodic subsystem code here (telemetry, safety checks) - m_poseEstimator.update( - pigeon.getRotation2d(), leftDis, rightDis); + m_poseEstimator.update( + pigeon.getRotation2d(), leftDis, rightDis); double leftPosition = leftPos - leftEncoder.getPosition() / 8.45 * wheelCircumference; double rightPosition = rightPos - rightEncoder.getPosition() / 8.45 * wheelCircumference; leftPos = leftEncoder.getPosition() / 8.45 * wheelCircumference; rightPos = rightEncoder.getPosition() / 8.45 * wheelCircumference; - + leftDis = leftDis + leftPosition; rightDis = rightDis + rightPosition; SmartDashboard.putNumber("Dis", rightDis); @@ -203,8 +229,8 @@ public void periodic() { SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); - Pose2d pose = m_poseEstimator.getEstimatedPosition(); + Pose2d pose = m_poseEstimator.getEstimatedPosition(); - field.setRobotPose(pose); + field.setRobotPose(pose); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cc31495..3bd7eb7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -61,6 +61,13 @@ public Command StartIntake() { }); } + + public void StartIntakeVoid(){ + IntakeMotor.set(Constants.Subsystems.Intake.kMaxIntakeSpeed); + } + + + public Command ReverseIntake() { return this.run(() -> { IntakeMotor.set(-Constants.Subsystems.Intake.kMaxIntakeSpeed); diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 1011f00..9c54dd2 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Constants.Subsystems.Shooter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.math.MathUtil; @@ -35,7 +36,7 @@ public class PhotonVision extends SubsystemBase { private PhotonCamera camera; PIDController turnPID = new PIDController(0.08, 0.0, 0); - PIDController drivePID = new PIDController(0.6, 0, 0); + PIDController drivePID = new PIDController(1, 0, 0.1); Rotation2d targetYaw; double distanceToTarget; double rotaioionSpeed; @@ -56,7 +57,7 @@ public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubs camera = new PhotonCamera("MainCamera"); turnPID.setTolerance(3); // degrees - drivePID.setTolerance(0.06); // meters + drivePID.setTolerance(0.1524); // meters photonEstimator = new PhotonPoseEstimator( Constants.Subsystems.Vision.kAprilTagFieldLayout, @@ -160,14 +161,11 @@ public Command AimShoot() { System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); if (turnPID.atSetpoint() && drivePID.atSetpoint()) { m_driveSubsystem.arcadeDrive(0, 0); - m_ShooterSubsystem.StartShoot(); - m_AgitatorSubsystem.StartAgitator(); - m_IntakeSubsystem.StartIntake(); - } else { - m_ShooterSubsystem.StopShoot(); - m_AgitatorSubsystem.StopAgitator(); - m_IntakeSubsystem.StopIntake(); - } + m_ShooterSubsystem.StartShootVoid(); + m_AgitatorSubsystem.StartAgitatorVoid(); + System.out.println("Shooting"); + m_IntakeSubsystem.StartIntakeVoid(); + } }, m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 39ec41d..cedc3f0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -77,24 +77,26 @@ public Command StartShoot() { }}); } + public void StartShootVoid(){ + Shooter1Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut1); + if (ShooterEncoder.getVelocity() > 2000){ + Shooter2Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut2); + } } + + + public Command StopShoot() { return this.run(() -> { - Shooter1Motor.set(0.0); - Shooter2Motor.set(0.0); - }); - } + - // Named command for PathPlanner - public Command shoot_1_Command() { - return this.run(() -> { - ShooterEncoder.setPosition(0.0); // Reset encoder position to 0 - if (ShooterEncoder.getPosition() < 42) { - StartShoot(); - } else { - StopShoot(); - } - }); + Shooter1Motor.set(0); + + Shooter2Motor.set(0); + + }); } + + } From 6d8bfc40971be22b74d445fac120f0d95206c93c Mon Sep 17 00:00:00 2001 From: Johnjacob12345 <162513125+Johnjacob12345@users.noreply.github.com> Date: Fri, 6 Mar 2026 20:05:41 -0800 Subject: [PATCH 02/10] Working Auto --- src/main/java/frc/robot/Robot.java | 5 ++++- .../frc/robot/subsystems/DriveSubsystem.java | 2 +- .../java/frc/robot/subsystems/PhotonVision.java | 16 +++++++++++++++- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d572ecf..3c3d864 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj.DriverStation; /** @@ -90,7 +91,8 @@ public void autonomousInit() { if (m_autonomousCommand != null) { CommandScheduler.getInstance().schedule(Commands.sequence( m_driveForwardCommand, - m_autonomousCommand) + new WaitCommand(2.0), // Wait 2 Seconds + m_autonomousCommand.repeatedly()) ); } } @@ -109,6 +111,7 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + m_driveForwardCommand.cancel(); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d218b6f..e04faac 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -139,7 +139,7 @@ public Command DriveForward() { } else { drive.arcadeDrive(0, 0); - + done = true; } diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 9c54dd2..6f89ffc 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -94,6 +94,10 @@ public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubs */ @Override public void periodic() { + RunCamera(); + } + + public void RunCamera(){ var result = camera.getLatestResult(); if (result.hasTargets()) { Optional visionEst = Optional.empty(); @@ -108,11 +112,17 @@ public void periodic() { m_driveSubsystem.addVisionMeasurement(VisionEst2d, Timer.getFPGATimestamp()); } } + + + } + public Command AimShoot() { return new RunCommand(() -> { + + RunCamera(); double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseBlue); Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), @@ -169,4 +179,8 @@ public Command AimShoot() { }, m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); - }} \ No newline at end of file + } + + + +} \ No newline at end of file From f173516e5908b4df54577ccc62826ea25b9ee468 Mon Sep 17 00:00:00 2001 From: Johnjacob12345 <162513125+Johnjacob12345@users.noreply.github.com> Date: Fri, 6 Mar 2026 19:53:02 -0800 Subject: [PATCH 03/10] Added PIDs for intake and shooter. Also improved auto align. --- src/main/java/frc/robot/Constants.java | 24 ++- src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/subsystems/IntakeSubsystem.java | 10 +- .../frc/robot/subsystems/PhotonVision.java | 143 +++++++++++++++++- .../robot/subsystems/ShooterSubsystem.java | 15 +- 5 files changed, 184 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f59af39..3c59f85 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -79,7 +79,7 @@ public static final class Intake { public static final class Vision { //Target - public static final double kYawTarget = -8; //Degrees + public static final double kYawTarget = -9; //Degrees public static final double kDistanceTarget = 1.9; //Meters public static final AprilTagFieldLayout kAprilTagFieldLayout = AprilTagFieldLayout .loadField(AprilTagFields.kDefaultField); @@ -93,6 +93,26 @@ public static final class Vision { Units.inchesToMeters(158.84), new Rotation2d(0.0)); + + + public static final Pose2d kClimbFinalRed = new Pose2d( + Units.inchesToMeters(608.5), + Units.inchesToMeters(146.22), + new Rotation2d(0.0)); + public static final Pose2d kClimbFinalBlue = new Pose2d( + Units.inchesToMeters(41.5), + Units.inchesToMeters(164.47), + new Rotation2d(0.0)); + public static final Pose2d kClimbFirstRed = new Pose2d( + Units.inchesToMeters(580.03), + Units.inchesToMeters(146.22), + new Rotation2d(0.0)); + public static final Pose2d kClimbFirstBlue = new Pose2d( + Units.inchesToMeters(79.33), + Units.inchesToMeters(164.47), + new Rotation2d(0.0)); + + public static final Transform3d kCameraToRobot = new Transform3d( -0.1016, // forward from robot center 0.0, // left/right camera is centered @@ -105,7 +125,7 @@ public static final class Agitator { // Ports public static final int kAgitatorPort = 7;//PWM // Speeds - public static final double kMaxAgitatorSpeed = 1.0; // Agitator Speed = 30% + public static final double kMaxAgitatorSpeed = 0.7; // Agitator Speed = 30% } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9b600e..1c368a9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ public class RobotContainer { private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); private final AgitatorSubsystem m_AgitatorSubsystem = new AgitatorSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); + private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem, m_ClimberSubsystem); // Replace with CommandPS4Controller or CommandJoystick if needed @@ -131,6 +131,9 @@ private void configureBindings() { .onTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake())); + // m_operatorController.b() + // .whileTrue(m_photonVision.AimClimb()) + // .onFalse(m_photonVision.resetClimb()); m_driverController.rightBumper().negate().and(m_driverController.rightTrigger().negate()) .onTrue(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake())); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 3bd7eb7..baad9c9 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,12 +4,15 @@ package frc.robot.subsystems; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; // For CAN import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; // For PWM @@ -18,7 +21,8 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ private SparkMax IntakeMotor = new SparkMax(Constants.Subsystems.Intake.kIntakePort, MotorType.kBrushless); - + private final RelativeEncoder intakeEncoder = IntakeMotor.getEncoder(); + PIDController speedPID = new PIDController(0.0008, 0.0005, 0.00005); public IntakeSubsystem() { @@ -53,11 +57,13 @@ public IntakeSubsystem() { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake", intakeEncoder.getVelocity()); } public Command StartIntake() { return this.run(() -> { - IntakeMotor.set(Constants.Subsystems.Intake.kMaxIntakeSpeed); + double intake = speedPID.calculate(intakeEncoder.getVelocity(), 900); + IntakeMotor.set(intake); }); } diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 6f89ffc..cd34e6f 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -31,15 +32,21 @@ public class PhotonVision extends SubsystemBase { private final ShooterSubsystem m_ShooterSubsystem; private final AgitatorSubsystem m_AgitatorSubsystem; private final IntakeSubsystem m_IntakeSubsystem; + private final ClimberSubsystem m_ClimberSubsystem; private PhotonPoseEstimator photonEstimator; private PhotonCamera camera; - PIDController turnPID = new PIDController(0.08, 0.0, 0); - PIDController drivePID = new PIDController(1, 0, 0.1); + PIDController turnPID = new PIDController(0.1, 0.005, 0.01); + PIDController drivePID = new PIDController(1.05, 0.005, 0.1); + PIDController climbDrivePID = new PIDController(1.05, 0.005, 0.1); + PIDController climbTurnPID = new PIDController(0.1, 0.005, 0.01); Rotation2d targetYaw; + Rotation2d climbTargetYaw; double distanceToTarget; + double climbDistanceToTarget; double rotaioionSpeed; + boolean climbFirst = false; /** * Construct PhotonVision with shared subsystem references. @@ -48,16 +55,19 @@ public class PhotonVision extends SubsystemBase { * @param shooter shared ShooterSubsystem * @param agitator shared AgitatorSubsystem */ - public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubsystem agitator, IntakeSubsystem intake) { + public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubsystem agitator, IntakeSubsystem intake, ClimberSubsystem climb) { this.m_driveSubsystem = drive; this.m_ShooterSubsystem = shooter; this.m_AgitatorSubsystem = agitator; this.m_IntakeSubsystem = intake; + this.m_ClimberSubsystem = climb; camera = new PhotonCamera("MainCamera"); turnPID.setTolerance(3); // degrees drivePID.setTolerance(0.1524); // meters + climbTurnPID.setTolerance(3); + climbDrivePID.setTolerance(Units.inchesToMeters(2)); photonEstimator = new PhotonPoseEstimator( Constants.Subsystems.Vision.kAprilTagFieldLayout, @@ -183,4 +193,131 @@ public Command AimShoot() { +// public Command AimClimb() { +// return new RunCommand(() -> { + +// if (!climbFirst){ + +// //Put climber up +// m_ClimberSubsystem.UpClimb(); + + +// double distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFirstBlue); +// Rotation2d targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFirstBlue); + + + + + +// var allianceOptional = DriverStation.getAlliance(); +// DriverStation.Alliance alliance = allianceOptional.get(); + +// if (alliance == DriverStation.Alliance.Red) { +// // Distance +// distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFirstRed); +// // Rotation +// targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFirstRed); + +// } else if (alliance == DriverStation.Alliance.Blue) { +// // Distance +// distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFirstBlue); + +// // Rotation +// targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFirstBlue); +// } else { +// System.out.println("Error loading Allance color"); +// } + +// System.out.println("Yaw" + targetYawFirst.getDegrees()); +// double climbRotaioionSpeed = turnPID.calculate(targetYawFirst.getDegrees(), 0); + +// double climbDriveSpeed = drivePID.calculate(distanceToFirstTarget, 0); + +// // Clamp to safty range +// climbRotaioionSpeed = MathUtil.clamp(climbRotaioionSpeed, -0.7, +// 0.7); +// climbDriveSpeed = MathUtil.clamp(climbDriveSpeed, -1, +// 1); + + +// if (!turnPID.atSetpoint()) { +// m_driveSubsystem.arcadeDrive(0, climbRotaioionSpeed); +// } else { +// m_driveSubsystem.arcadeDrive(climbDriveSpeed, 0); +// } + +// System.out.println("Turn: " + climbTurnPID.atSetpoint() + "Drive" + climbDrivePID.atSetpoint()); +// if (climbTurnPID.atSetpoint() && climbDrivePID.atSetpoint()) { +// climbFirst = true; +// } + + +// } else { + + +// double distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFinalBlue); +// Rotation2d targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFinalBlue); + + + +// var allianceOptional = DriverStation.getAlliance(); +// DriverStation.Alliance alliance = allianceOptional.get(); + +// if (alliance == DriverStation.Alliance.Red) { +// // Distance +// distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFinalRed); +// // Rotation +// targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFinalRed); + +// } else if (alliance == DriverStation.Alliance.Blue) { +// // Distance +// distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), +// Constants.Subsystems.Vision.kClimbFinalBlue); + +// // Rotation +// targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFinalBlue); +// } else { +// System.out.println("Error loading Allance color"); +// } + + +// double climbRotaioionSpeed = turnPID.calculate(targetYawFinal.getDegrees(), 0); + +// double climbDriveSpeed = drivePID.calculate(distanceToFinalTarget, 0); + +// // Clamp to safty range +// climbRotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, +// 0.7); +// climbDriveSpeed = MathUtil.clamp(climbDriveSpeed, -1, +// 1); + + +// if (!turnPID.atSetpoint()) { +// m_driveSubsystem.arcadeDrive(0, climbRotaioionSpeed); +// } else { +// m_driveSubsystem.arcadeDrive(climbDriveSpeed, 0); +// } + +// System.out.println("Turn: " + climbTurnPID.atSetpoint() + "Drive" + climbDrivePID.atSetpoint()); +// if (climbTurnPID.atSetpoint() && climbDrivePID.atSetpoint()) { +// m_ClimberSubsystem.DownClimb(); +// } +// } +// }, m_driveSubsystem, m_ClimberSubsystem); +// } + +// public Command resetClimb(){ +// return this.run(() -> { +// climbFirst = false; +// }); +// } + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index cedc3f0..9a9cd9e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.controller.PIDController; + // For CAN import com.revrobotics.spark.SparkMax; @@ -24,7 +26,7 @@ public class ShooterSubsystem extends SubsystemBase { private SparkMax Shooter1Motor = new SparkMax(Constants.Subsystems.Shooter.kShooter1Port, MotorType.kBrushless); private PWMVictorSPX Shooter2Motor = new PWMVictorSPX(Constants.Subsystems.Shooter.kShooter2Port); private final RelativeEncoder ShooterEncoder = Shooter1Motor.getEncoder(); - // private final PIDController pid = new PIDController(0, 0.0, 0.0); + PIDController shooterPID = new PIDController(0.00027, 0.00017, 0.000017); public ShooterSubsystem() { @@ -69,16 +71,19 @@ public void periodic() { public Command StartShoot() { return this.run(() -> { + double shooter = shooterPID.calculate(ShooterEncoder.getVelocity(), 5000); + - - Shooter1Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut1); - if (ShooterEncoder.getVelocity() > 2000){ + Shooter1Motor.set(shooter); + if (ShooterEncoder.getVelocity() > 4000){ Shooter2Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut2); }}); } public void StartShootVoid(){ - Shooter1Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut1); + + double shooter = shooterPID.calculate(ShooterEncoder.getVelocity(), 5000); + Shooter1Motor.set(shooter); if (ShooterEncoder.getVelocity() > 2000){ Shooter2Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut2); } } From 3e9bd4e8eebae221ecb8be0f6f7401fa35923cba Mon Sep 17 00:00:00 2001 From: DevWizard Date: Thu, 12 Mar 2026 19:12:04 -0700 Subject: [PATCH 04/10] Added climbing support --- src/main/java/frc/robot/Constants.java | 9 +++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/PhotonVision.java | 62 ++++++++++++++++++- 3 files changed, 70 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ebfd9b2..485854f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -93,6 +93,15 @@ public static final class Vision { Units.inchesToMeters(158.84), new Rotation2d(0.0)); + public static final Pose2d blueClimbPos = new Pose2d( + Units.inchesToMeters(182.11), + Units.inchesToMeters(158.84), + new Rotation2d(0.0)); + public static final Pose2d redClimbPos = new Pose2d( + Units.inchesToMeters(469.64), + Units.inchesToMeters(158.84), + new Rotation2d(0.0)); + public static final Transform3d kCameraToRobot = new Transform3d( -0.1016, // forward from robot center 0.0, // left/right camera is centered diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc6923c..48b27ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ public class RobotContainer { private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); private final AgitatorSubsystem m_AgitatorSubsystem = new AgitatorSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem); + private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_ClimberSubsystem); // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(Driver.kJoystickID); diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index b798188..c3fbeab 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -35,6 +35,7 @@ public class PhotonVision extends SubsystemBase { private final DriveSubsystem m_driveSubsystem; private final ShooterSubsystem m_ShooterSubsystem; + private final ClimberSubsystem m_climberSubsystem; private PhotonPoseEstimator photonEstimator; private PhotonCamera camera; @@ -51,9 +52,10 @@ public class PhotonVision extends SubsystemBase { * @param drive shared DriveSubsystem * @param shooter shared ShooterSubsystem */ - public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter) { + public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, ClimberSubsystem climber) { this.m_driveSubsystem = drive; this.m_ShooterSubsystem = shooter; + this.m_climberSubsystem = climber; camera = new PhotonCamera("MainCamera"); @@ -108,6 +110,61 @@ public void periodic() { } } + public Command AimClimb() { + return new RunCommand(() -> { + + double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + + var allianceOptional = DriverStation.getAlliance(); + DriverStation.Alliance alliance = allianceOptional.get(); + + if (alliance == DriverStation.Alliance.Red) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.redClimbPos); + System.out.println("Distance to Target: " + distanceToTarget); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); + } else if (alliance == DriverStation.Alliance.Blue) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.blueClimbPos); + System.out.println("Distance to Target: " + distanceToTarget); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); + } else { + System.out.println("Error loading Allance color"); + } + + System.out.println("Yaw" + targetYaw.getDegrees()); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); + + double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); + + // Clamp to safty range + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -Constants.Subsystems.Drive.kMaxNormalSpeed, + Constants.Subsystems.Drive.kMaxNormalSpeed); + driveSpeed = MathUtil.clamp(driveSpeed, -Constants.Subsystems.Drive.kMaxRotSpeed, + Constants.Subsystems.Drive.kMaxRotSpeed); + + if (!turnPID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); + } else { + m_driveSubsystem.arcadeDrive(driveSpeed, 0); + } + + System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); + if (turnPID.atSetpoint()) { + m_climberSubsystem.UpClimb(); + } else { + m_climberSubsystem.DownClimb(); + } + }, m_driveSubsystem, m_climberSubsystem); + } + public Command AimShoot() { return new RunCommand(() -> { @@ -188,4 +245,5 @@ public Command AimShoot() { // } }, m_driveSubsystem, m_ShooterSubsystem); - }} \ No newline at end of file + } +} From 67c8de9899a80c6476f32bedb3779a3faea0d3d8 Mon Sep 17 00:00:00 2001 From: DevWizard Date: Fri, 13 Mar 2026 13:09:13 -0700 Subject: [PATCH 05/10] Bound PhotonVision.AimClimb() to the left bumper in RobotContainer TLDR, push left bumper to climb. --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48b27ae..0d6db1d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,6 +123,9 @@ private void configureBindings() { // Shooter control m_driverController.rightBumper() .whileTrue(m_photonVision.AimShoot()); + + m_driverController.leftBumber() + .whileTrue(m_photonVision.AimClimb()); // Start Shooter (constant speed) m_driverController.rightTrigger() From ff8c0e042a2072b9dc9df2c5d5a155d2798cce44 Mon Sep 17 00:00:00 2001 From: DevWizard Date: Mon, 16 Mar 2026 16:40:06 -0700 Subject: [PATCH 06/10] FINALLY set the position of the climber I'm really sorry it took this long. It still needs testing. I did account for the offset of the climber. --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 485854f..9f894ec 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -94,12 +94,12 @@ public static final class Vision { new Rotation2d(0.0)); public static final Pose2d blueClimbPos = new Pose2d( - Units.inchesToMeters(182.11), - Units.inchesToMeters(158.84), + Units.inchesToMeters(163.86), + Units.inchesToMeters(21.75), new Rotation2d(0.0)); public static final Pose2d redClimbPos = new Pose2d( - Units.inchesToMeters(469.64), - Units.inchesToMeters(158.84), + Units.inchesToMeters(649.57 - (29.5 / 2)), + Units.inchesToMeters(152.78), new Rotation2d(0.0)); public static final Transform3d kCameraToRobot = new Transform3d( From 3fe6e0a3bf53565bef91dde0ca85ee46cdb137f2 Mon Sep 17 00:00:00 2001 From: DevWizard Date: Mon, 16 Mar 2026 17:38:34 -0700 Subject: [PATCH 07/10] Changed some values Mainly the distance that it needs to be away from the place (0m) and the rotation (0 and 180) --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/subsystems/PhotonVision.java | 92 ++++++++++--------- 3 files changed, 53 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9f894ec..4ccf12a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,11 +95,11 @@ public static final class Vision { public static final Pose2d blueClimbPos = new Pose2d( Units.inchesToMeters(163.86), - Units.inchesToMeters(21.75), + Units.inchesToMeters(21.75 + 11.46), new Rotation2d(0.0)); public static final Pose2d redClimbPos = new Pose2d( - Units.inchesToMeters(649.57 - (29.5 / 2)), - Units.inchesToMeters(152.78), + Units.inchesToMeters(649.57), + Units.inchesToMeters(152.78 - 11.46), new Rotation2d(0.0)); public static final Transform3d kCameraToRobot = new Transform3d( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d6db1d..52c245d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,8 +124,8 @@ private void configureBindings() { m_driverController.rightBumper() .whileTrue(m_photonVision.AimShoot()); - m_driverController.leftBumber() - .whileTrue(m_photonVision.AimClimb()); + //m_driverController.leftBumber() + // .whileTrue(m_photonVision.AimClimb()); // Start Shooter (constant speed) m_driverController.rightTrigger() diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index c3fbeab..87cb621 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -113,55 +113,59 @@ public void periodic() { public Command AimClimb() { return new RunCommand(() -> { - double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - - var allianceOptional = DriverStation.getAlliance(); - DriverStation.Alliance alliance = allianceOptional.get(); - - if (alliance == DriverStation.Alliance.Red) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.redClimbPos); - System.out.println("Distance to Target: " + distanceToTarget); - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); - } else if (alliance == DriverStation.Alliance.Blue) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.blueClimbPos); - System.out.println("Distance to Target: " + distanceToTarget); - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); - } else { - System.out.println("Error loading Allance color"); - } + double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + + var allianceOptional = DriverStation.getAlliance(); + DriverStation.Alliance alliance = allianceOptional.get(); + + double yawTarget = 0.0; + + if (alliance == DriverStation.Alliance.Red) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.redClimbPos); + System.out.println("Distance to Target: " + distanceToTarget); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); + yawTarget = 180.0; + } else if (alliance == DriverStation.Alliance.Blue) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.blueClimbPos); + System.out.println("Distance to Target: " + distanceToTarget); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); + yawTarget = 0.0; + } else { + System.out.println("Error loading Allance color"); + } - System.out.println("Yaw" + targetYaw.getDegrees()); - double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); + System.out.println("Yaw" + targetYaw.getDegrees()); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); - double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); + double driveSpeed = drivePID.calculate(distanceToTarget, 0.0); - // Clamp to safty range - rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -Constants.Subsystems.Drive.kMaxNormalSpeed, - Constants.Subsystems.Drive.kMaxNormalSpeed); - driveSpeed = MathUtil.clamp(driveSpeed, -Constants.Subsystems.Drive.kMaxRotSpeed, - Constants.Subsystems.Drive.kMaxRotSpeed); + // Clamp to safty range + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -Constants.Subsystems.Drive.kMaxNormalSpeed, + Constants.Subsystems.Drive.kMaxNormalSpeed); + driveSpeed = MathUtil.clamp(driveSpeed, -Constants.Subsystems.Drive.kMaxRotSpeed, + Constants.Subsystems.Drive.kMaxRotSpeed); - if (!turnPID.atSetpoint()) { - m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); - } else { - m_driveSubsystem.arcadeDrive(driveSpeed, 0); - } + if (!turnPID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); + } else { + m_driveSubsystem.arcadeDrive(driveSpeed, 0); + } - System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); - if (turnPID.atSetpoint()) { - m_climberSubsystem.UpClimb(); - } else { - m_climberSubsystem.DownClimb(); - } + System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); + if (turnPID.atSetpoint()) { + m_climberSubsystem.UpClimb(); + } else { + m_climberSubsystem.DownClimb(); + } }, m_driveSubsystem, m_climberSubsystem); } From ffa7c9a5ca9ce79c547e545f15d8494f8019688a Mon Sep 17 00:00:00 2001 From: DevWizard Date: Tue, 17 Mar 2026 17:03:11 -0700 Subject: [PATCH 08/10] Added climber support Using the updated code this time, not the old one. --- .../frc/robot/subsystems/PhotonVision.java | 285 ++++-------------- 1 file changed, 58 insertions(+), 227 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index ba5d41f..eadb42e 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -74,34 +74,6 @@ public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubs Constants.Subsystems.Vision.kCameraToRobot); } - - - - /** - * Example command factory method. - * - * @return a command - * - * public Command exampleMethodCommand() { - * // Inline construction of command goes here. - * // Subsystem::RunOnce implicitly requires `this` subsystem. - * return runOnce( - * () -> { - * one-time action goes here - * }); - * } - * - * /** - * An example method querying a boolean state of the subsystem (for - * example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - * - * public boolean exampleCondition() { - * // Query some boolean state, such as a digital sensor. - * return false; - * } - */ @Override public void periodic() { RunCamera(); @@ -122,54 +94,45 @@ public void RunCamera(){ m_driveSubsystem.addVisionMeasurement(VisionEst2d, Timer.getFPGATimestamp()); } } - - - } public Command AimClimb() { return new RunCommand(() -> { - + RunCamera(); double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); + Constants.Subsystems.Vision.blueClimbPos); Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); + Constants.Subsystems.Vision.blueClimbPos); var allianceOptional = DriverStation.getAlliance(); DriverStation.Alliance alliance = allianceOptional.get(); - double yawTarget = 0.0; - if (alliance == DriverStation.Alliance.Red) { // Distance distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); - System.out.println("Distance to Target: " + distanceToTarget); // Rotation targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); - yawTarget = 180.0; + } else if (alliance == DriverStation.Alliance.Blue) { // Distance distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); - System.out.println("Distance to Target: " + distanceToTarget); + // Rotation targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); - yawTarget = 0.0; } else { System.out.println("Error loading Allance color"); } System.out.println("Yaw" + targetYaw.getDegrees()); - double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), 0); - double driveSpeed = drivePID.calculate(distanceToTarget, 0.0); + double driveSpeed = drivePID.calculate(distanceToTarget, 0); // Clamp to safty range - rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -Constants.Subsystems.Drive.kMaxNormalSpeed, - Constants.Subsystems.Drive.kMaxNormalSpeed); - driveSpeed = MathUtil.clamp(driveSpeed, -Constants.Subsystems.Drive.kMaxRotSpeed, - Constants.Subsystems.Drive.kMaxRotSpeed); + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, 0.7); + driveSpeed = MathUtil.clamp(driveSpeed, -1, 1); if (!turnPID.atSetpoint()) { m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); @@ -178,202 +141,70 @@ public Command AimClimb() { } System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); - if (turnPID.atSetpoint()) { + if (turnPID.atSetpoint() && drivePID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, 0); m_ClimberSubsystem.UpClimb(); - } else { - m_ClimberSubsystem.DownClimb(); + System.out.println("Climbing"); } }, m_driveSubsystem, m_ClimberSubsystem); } public Command AimShoot() { return new RunCommand(() -> { - - - RunCamera(); - double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - - var allianceOptional = DriverStation.getAlliance(); - DriverStation.Alliance alliance = allianceOptional.get(); - - if (alliance == DriverStation.Alliance.Red) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseRed); - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseRed); - - } else if (alliance == DriverStation.Alliance.Blue) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseBlue); - } else { - System.out.println("Error loading Allance color"); - } - - System.out.println("Yaw" + targetYaw.getDegrees()); - double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); - - double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); - - - // Clamp to safty range - rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, - 0.7); - driveSpeed = MathUtil.clamp(driveSpeed, -1, - 1); - - - if (!turnPID.atSetpoint()) { - m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); - } else { - m_driveSubsystem.arcadeDrive(driveSpeed, 0); - } - - System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); - if (turnPID.atSetpoint() && drivePID.atSetpoint()) { - m_driveSubsystem.arcadeDrive(0, 0); - m_ShooterSubsystem.StartShootVoid(); - m_AgitatorSubsystem.StartAgitatorVoid(); - System.out.println("Shooting"); - m_IntakeSubsystem.StartIntakeVoid(); - } - - - }, m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); - } - - - -// public Command AimClimb() { -// return new RunCommand(() -> { - -// if (!climbFirst){ - -// //Put climber up -// m_ClimberSubsystem.UpClimb(); - - -// double distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFirstBlue); -// Rotation2d targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFirstBlue); - - - - + RunCamera(); + double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); -// var allianceOptional = DriverStation.getAlliance(); -// DriverStation.Alliance alliance = allianceOptional.get(); + var allianceOptional = DriverStation.getAlliance(); + DriverStation.Alliance alliance = allianceOptional.get(); -// if (alliance == DriverStation.Alliance.Red) { -// // Distance -// distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFirstRed); -// // Rotation -// targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFirstRed); + if (alliance == DriverStation.Alliance.Red) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseRed); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseRed); -// } else if (alliance == DriverStation.Alliance.Blue) { -// // Distance -// distanceToFirstTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFirstBlue); + } else if (alliance == DriverStation.Alliance.Blue) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); -// // Rotation -// targetYawFirst = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFirstBlue); -// } else { -// System.out.println("Error loading Allance color"); -// } + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseBlue); + } else { + System.out.println("Error loading Allance color"); + } -// System.out.println("Yaw" + targetYawFirst.getDegrees()); -// double climbRotaioionSpeed = turnPID.calculate(targetYawFirst.getDegrees(), 0); + System.out.println("Yaw" + targetYaw.getDegrees()); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); -// double climbDriveSpeed = drivePID.calculate(distanceToFirstTarget, 0); + double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); -// // Clamp to safty range -// climbRotaioionSpeed = MathUtil.clamp(climbRotaioionSpeed, -0.7, -// 0.7); -// climbDriveSpeed = MathUtil.clamp(climbDriveSpeed, -1, -// 1); - - -// if (!turnPID.atSetpoint()) { -// m_driveSubsystem.arcadeDrive(0, climbRotaioionSpeed); -// } else { -// m_driveSubsystem.arcadeDrive(climbDriveSpeed, 0); -// } - -// System.out.println("Turn: " + climbTurnPID.atSetpoint() + "Drive" + climbDrivePID.atSetpoint()); -// if (climbTurnPID.atSetpoint() && climbDrivePID.atSetpoint()) { -// climbFirst = true; -// } + // Clamp to safty range + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, + 0.7); + driveSpeed = MathUtil.clamp(driveSpeed, -1, + 1); -// } else { - - -// double distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFinalBlue); -// Rotation2d targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFinalBlue); - - - -// var allianceOptional = DriverStation.getAlliance(); -// DriverStation.Alliance alliance = allianceOptional.get(); - -// if (alliance == DriverStation.Alliance.Red) { -// // Distance -// distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFinalRed); -// // Rotation -// targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFinalRed); - -// } else if (alliance == DriverStation.Alliance.Blue) { -// // Distance -// distanceToFinalTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), -// Constants.Subsystems.Vision.kClimbFinalBlue); - -// // Rotation -// targetYawFinal = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kClimbFinalBlue); -// } else { -// System.out.println("Error loading Allance color"); -// } - - -// double climbRotaioionSpeed = turnPID.calculate(targetYawFinal.getDegrees(), 0); + + if (!turnPID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); + } else { + m_driveSubsystem.arcadeDrive(driveSpeed, 0); + } -// double climbDriveSpeed = drivePID.calculate(distanceToFinalTarget, 0); - -// // Clamp to safty range -// climbRotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, -// 0.7); -// climbDriveSpeed = MathUtil.clamp(climbDriveSpeed, -1, -// 1); - - -// if (!turnPID.atSetpoint()) { -// m_driveSubsystem.arcadeDrive(0, climbRotaioionSpeed); -// } else { -// m_driveSubsystem.arcadeDrive(climbDriveSpeed, 0); -// } - -// System.out.println("Turn: " + climbTurnPID.atSetpoint() + "Drive" + climbDrivePID.atSetpoint()); -// if (climbTurnPID.atSetpoint() && climbDrivePID.atSetpoint()) { -// m_ClimberSubsystem.DownClimb(); -// } -// } -// }, m_driveSubsystem, m_ClimberSubsystem); -// } - -// public Command resetClimb(){ -// return this.run(() -> { -// climbFirst = false; -// }); -// } + System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); + if (turnPID.atSetpoint() && drivePID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, 0); + m_ShooterSubsystem.StartShootVoid(); + m_AgitatorSubsystem.StartAgitatorVoid(); + System.out.println("Shooting"); + m_IntakeSubsystem.StartIntakeVoid(); + } + }, m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); + } } From 37d771807b7dbfaaa64dd23eb7efb7c271dbc5f8 Mon Sep 17 00:00:00 2001 From: DevWizard Date: Wed, 18 Mar 2026 16:46:14 -0700 Subject: [PATCH 09/10] Added Button Binding It's the left bumper on the driver controller. I didn't add it last time because it gave an error when I tried and I forgot to add it back after. --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14c5e25..acd1e63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,6 +123,9 @@ private void configureBindings() { // Shooter control m_driverController.rightBumper() .onTrue(m_photonVision.AimShoot()); + + m_driverController.leftBumper() + .onTrue(m_photonVision.AimClimb()); // Start Shooter (constant speed) m_driverController.rightTrigger() From b1c67fd8994c1a7e6c89194a4f2ac65ef89f8153 Mon Sep 17 00:00:00 2001 From: DevWizard Date: Wed, 18 Mar 2026 16:48:54 -0700 Subject: [PATCH 10/10] Changed the climb binding from the driver to the operator controller --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index acd1e63..6dd4811 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,7 +124,7 @@ private void configureBindings() { m_driverController.rightBumper() .onTrue(m_photonVision.AimShoot()); - m_driverController.leftBumper() + m_operatorController.leftBumper() .onTrue(m_photonVision.AimClimb()); // Start Shooter (constant speed)