diff --git a/lib/RoboLib b/lib/RoboLib index eeccb75..ad53b44 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee +Subproject commit ad53b44e2fb4c06b72ff2546b30e37446ac2fb8d diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a043bba..34f7d79 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,9 +22,16 @@ public final class Constants { } public static class CameraConstanst { + + public static class IDFilters { + public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 }; + public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 }; + } + public static final String BACK_CAMERA_NAME = "back"; public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0; - public static final int BACK_HUB_TRACKING_PIPELINE_NUMBER = 1; + public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1; + public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2; public static final String FRONT_CAMERA_NAME = "front"; public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f55b897..08dd26a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,6 +60,7 @@ public class RobotContainer { private void configureBindings() { turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID())); hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID())); + intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); @@ -69,9 +70,8 @@ public class RobotContainer { m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); - m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, intakeSubsystem, turretSubsystem)); + m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); - intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); } /** diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 18a873a..c29196a 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -1,16 +1,20 @@ package frc.robot.commands; -import java.lang.constant.Constable; +import java.util.Arrays; +import java.util.List; import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; -import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers.LimelightTarget_Fiducial; - +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; public class AutoShootCommandChassisTurret extends Command{ @@ -19,41 +23,67 @@ public class AutoShootCommandChassisTurret extends Command{ private DriveSubsystem driveSubsystem; private HopperSubsystem hopperSubsystem; - private IntakeSubsystem intakeSubsystem; private TurretSubsystem turretSubsystem; private PIDController autoCenterController; - public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, IntakeSubsystem intakeSubsystem, TurretSubsystem turretSubsystem) + private int[] tagFilters; + + private AprilTagFieldLayout filedTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + + private final double chassisRotateSpeed = Math.PI * 0.1; + + public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { this.driveSubsystem = driveSubsystem; this.hopperSubsystem = hopperSubsystem; - this.intakeSubsystem = intakeSubsystem; this.turretSubsystem = turretSubsystem; + this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); + autoCenterController = new PIDController(0.1, 0, 0); - autoCenterController.setTolerance(1); + autoCenterController.setTolerance(1); // Units is pixels autoCenterController.setSetpoint(0); + + tagFilters = DriverStation.getAlliance().get() == Alliance.Blue ? Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS : Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; } @Override public void initialize() { turretSubsystem.setLauncher(2500); hopperSubsystem.spindexerOn(); + + // Change to HUB tracking pipeline + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); + LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); + } @Override public void execute() { // Need to compute shoot speed and angle - // Get apriltags - // Change to HUB tracking pipeline - LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_PIPELINE_NUMBER); + // No april tag in camera view. Rotate towards our theoretical closest tag + if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ + Pose3d botPose = driveSubsystem.getBotPose(); + // Find closest target to us + List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList(); + for(int i=0;i 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this + } + } + else // We have an april tag, center it to the camera frame + { + // [tx, ty, tz, pitch, yaw, roll] + double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); - LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); - - // driveSubsystem. autoCenterController.calculate(0); + driveSubsystem.rotateChassis(autoCenterController.calculate(targetsPose[0])); // This may need to be negated + } + // If robot is centered on HUB and shooter is up to speed. Run the kicker if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){ hopperSubsystem.feedKicker(); } @@ -71,6 +101,8 @@ public class AutoShootCommandChassisTurret extends Command{ hopperSubsystem.stopKicker(); hopperSubsystem.spindexerOff(); + driveSubsystem.stopDrive(); + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER); } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 107aad7..90030cd 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,15 +4,7 @@ package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; -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(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } private Autos() { throw new UnsupportedOperationException("This is a utility class!"); diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 7481d3c..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java index 21fadab..1c552f7 100644 --- a/src/main/java/frc/robot/commands/ManualHopperCommand.java +++ b/src/main/java/frc/robot/commands/ManualHopperCommand.java @@ -4,7 +4,6 @@ package frc.robot.commands; -import frc.robot.Constants; import frc.robot.subsystems.HopperSubsystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/ManualIntake.java b/src/main/java/frc/robot/commands/ManualIntake.java index a23181c..39c9aa4 100644 --- a/src/main/java/frc/robot/commands/ManualIntake.java +++ b/src/main/java/frc/robot/commands/ManualIntake.java @@ -6,8 +6,6 @@ package frc.robot.commands; import frc.robot.subsystems.IntakeSubsystem; -import javax.lang.model.util.ElementScanner14; - import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index 054940f..3225c01 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.Command; public class ManualShootCommand extends Command { private final TurretSubsystem m_subsystem; private final XboxController m_controller; - private double tempAng; public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) { m_subsystem = subsystem; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index ed762c7..b8a831c 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -6,15 +6,18 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; import org.littletonrobotics.junction.Logger; -import com.fasterxml.jackson.annotation.JsonCreator.Mode; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -129,6 +132,10 @@ public class DriveSubsystem extends TestableSubsystem { .build()) .build(); + + // Set vision measurement confidence values + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); + this.setDefaultCommand(Commands.run(new Runnable() { @Override @@ -169,9 +176,9 @@ public class DriveSubsystem extends TestableSubsystem { }, this); - // autoChooser = AutoBuilder.buildAutoChooser(); - // autoChooser.addOption("Do Nothing", Commands.none()); - // SmartDashboard.putData("Auto Chooser", autoChooser); + autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.setDefaultOption("Do Nothing", Commands.none()); + SmartDashboard.putData("Auto Chooser", autoChooser); } catch (Exception e) { e.printStackTrace(); @@ -183,6 +190,15 @@ public class DriveSubsystem extends TestableSubsystem { public void periodic() { super.periodic(); driveSystem.periodic(); + + LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); + if(frontCameraPose.tagCount >= 2){ + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); + } + LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); + if(backCameraPose.tagCount >= 2){ + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); + } } @Override @@ -192,8 +208,19 @@ public class DriveSubsystem extends TestableSubsystem { } public Command getAutoCommand(){ - return Commands.none(); - // return autoChooser.getSelected(); + return autoChooser.getSelected(); + } + + public void stopDrive(){ + driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); + } + + public void rotateChassis(double rotSpeed){ + driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); + } + + public Pose3d getBotPose() { + return driveSystem.getRobotPose3d(); } @PostTest() diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index e741555..a8bae7d 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -10,11 +10,7 @@ import org.fairportrobotics.frc.posty.test.PostTest; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; -import edu.wpi.first.wpilibj2.command.Command; - import static org.fairportrobotics.frc.posty.assertions.Assertions.*; -import static org.fairportrobotics.frc.robolib.motors.Utils.*; import frc.robot.Constants; public class HopperSubsystem extends TestableSubsystem { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 3fe9223..a200006 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -17,8 +17,6 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.robolib.motors.Utils.*; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */