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

Auto Chassis align works

This commit is contained in:
Tyler Wilcox 2026-03-23 21:03:00 -04:00
parent edf6c209bb
commit a6449604ec
5 changed files with 42 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,14 +26,15 @@ public final class Constants {
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 int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 18, 19, 20, 21 };
}
public static final String BACK_CAMERA_NAME = "back";
public static final String BACK_CAMERA_NAME = "limelight-back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
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 String FRONT_CAMERA_NAME = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
}
@ -101,7 +102,7 @@ public final class Constants {
public static final double TURRET_GEAR_RATIO = 1;
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
public static final int HOOD_ENCODER_ID = 4;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);

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

@ -58,17 +58,17 @@ 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()));
// 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));
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
// m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
// m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));

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

@ -4,9 +4,12 @@ import java.util.Arrays;
import java.util.List;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
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.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj.DriverStation;
@ -41,15 +44,21 @@ public class AutoShootCommandChassisTurret extends Command{
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
autoCenterController = new PIDController(0.1, 0, 0);
autoCenterController.setTolerance(1); // Units is pixels
autoCenterController = new PIDController(0.1, 0, 0.0);
autoCenterController.setTolerance(0.2); // 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() {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
} else {
tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
}
});
turretSubsystem.setLauncher(2500);
hopperSubsystem.spindexerOn();
@ -63,8 +72,11 @@ public class AutoShootCommandChassisTurret extends Command{
public void execute() {
// Need to compute shoot speed and angle
Logger.recordOutput("AutoAlign-CenteringError", autoCenterController.getError());
// No april tag in camera view. Rotate towards our theoretical closest tag
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
Logger.recordOutput("AutoAlignState", "Look for tag");
Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList();
@ -77,14 +89,17 @@ public class AutoShootCommandChassisTurret extends Command{
}
else // We have an april tag, center it to the camera frame
{
Logger.recordOutput("AutoAlignState", "Center on tag");
// [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
driveSubsystem.rotateChassis(autoCenterController.calculate(targetsPose[0])); // This may need to be negated
if(targetsPose.length >= 1){
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()){
Logger.recordOutput("AutoAlignState", "FIRE!");
hopperSubsystem.feedKicker();
}
}

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

@ -15,6 +15,7 @@ import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@ -134,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem {
// Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
// driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7));
this.setDefaultCommand(Commands.run(new Runnable() {
@ -191,13 +192,17 @@ public class DriveSubsystem extends TestableSubsystem {
super.periodic();
driveSystem.periodic();
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
if(frontCameraPose.tagCount >= 2){
if(frontCameraPose.tagCount >= 1){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
}
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(backCameraPose.tagCount >= 2){
if(backCameraPose.tagCount >= 1){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
}
}

9
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -27,6 +27,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants;
import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DigitalInput;
@ -106,12 +107,8 @@ public class TurretSubsystem extends TestableSubsystem {
public void setTargetElevation(Angle elev)
{
if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){
elev=Units.Degrees.of(300).minus(elev);
}
elev=elev.div(1.5);
elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO);
hood.setAngle(elev.in(Units.Degrees));
double angle = elev.in(Units.Degrees);
hood.setAngle(300/angle);
}
public Command revSpeedCommand()