FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

More auto shoot

This commit is contained in:
Tyler Wilcox 2026-03-22 21:24:56 -04:00
parent ab9f726bc2
commit edf6c209bb
12 changed files with 90 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee
Subproject commit ad53b44e2fb4c06b72ff2546b30e37446ac2fb8d

9
src/main/java/frc/robot/Constants.java
View File

@ -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;

4
src/main/java/frc/robot/RobotContainer.java
View File

@ -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()));
}
/**

60
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -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<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList();
for(int i=0;i<tagFilters.length; i++)
{
Pose3d closet = botPose.nearest(tagPoses);
Transform3d delta = botPose.minus(closet);
driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 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);
}

8
src/main/java/frc/robot/commands/Autos.java
View File

@ -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!");

43
src/main/java/frc/robot/commands/ExampleCommand.java
View File

@ -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;
}
}

1
src/main/java/frc/robot/commands/ManualHopperCommand.java
View File

@ -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;

2
src/main/java/frc/robot/commands/ManualIntake.java
View File

@ -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;

1
src/main/java/frc/robot/commands/ManualShootCommand.java
View File

@ -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;

39
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -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()

4
src/main/java/frc/robot/subsystems/HopperSubsystem.java
View File

@ -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 {

2
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@ -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. */