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

Zone check? I dunno, tired :3

This commit is contained in:
TheRealGrayden 2026-04-09 20:28:28 -04:00
parent 2ad0eedff5
commit ad65dc07ce
5 changed files with 216 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,151 +1,167 @@
// Copyright (c) FIRST and other WPILib contributors. // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc.robot; package frc.robot;
import com.ctre.phoenix6.signals.InvertedValue;
import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.units.Units; import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
import com.ctre.phoenix6.signals.InvertedValue;
/** /**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * The Constants class provides a convenient place for teams to hold robot-wide
* constants. This class should not be used for any other purpose. All constants should be declared * numerical or boolean constants. This class should not be used for any other
* globally (i.e. public static). Do not put anything functional in this class. * purpose. All constants should be declared globally (i.e. public static). Do
* not put anything functional in this class.
* *
* <p>It is advised to statically import this class (or one of its inner classes) wherever the * <p>
* constants are needed, to reduce verbosity. * It is advised to statically import this class (or one of its inner classes)
* wherever the constants are needed, to reduce verbosity.
*/ */
public final class Constants { public final class Constants {
public static class OperatorConstants {
public static final int kDriverControllerPort = 0;
}
public static class CameraConstanst { public static class OperatorConstants {
public static class IDFilters { public static final int kDriverControllerPort = 0;
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 = "limelight-back"; public static class CameraConstanst {
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 = "limelight-front"; public static class IDFilters {
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
}
public static class AdvantageKitConstants{ public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
public enum RobotType{ public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
SIM, }
REAL,
REPLAY 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 = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final double[] BLUE_ZONE_COOR = new double[]{1};
public static final double[] RED_ZONE_COOR = new double[]{1};
public static final double[] NEUTRAL_ZONE_COOR = new double[]{1};
} }
public static final RobotType CURRENT_MODE = RobotType.REAL;
} public static class AdvantageKitConstants {
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets { public enum RobotType {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f; SIM,
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f; REAL,
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f; REPLAY
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f; }
} public static final RobotType CURRENT_MODE = RobotType.REAL;
/* }
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
}
/*
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
* Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot * Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
* Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot * Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
* Laters numbers will be added for more subsystems * Laters numbers will be added for more subsystems
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
*/ */
public static class SwerveDriveIDs { public static class SwerveDriveIDs {
public static final int FRONT_LEFT_STEER_ID = 11;
public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final int FRONT_LEFT_ENCODER_ID = 13;
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
public static class RobotChassisLimits{ public static final int FRONT_LEFT_STEER_ID = 11;
// Max velocity in meters per second public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; public static final int FRONT_LEFT_ENCODER_ID = 13;
// In meters per second squared public static final int FRONT_RIGHT_STEER_ID = 21;
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
// In radians per second public static class RobotChassisLimits {
public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; // Max velocity in meters per second
// In radians per second squared
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
}
//Bird public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
public static class ExtraIDEntities { // In meters per second squared
public static final int PIGEON_ID = 1; public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
}
public static class IntakeConstants {
public static final int DEPLOY_MOTOR_ID = 17;
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
public static final int INTAKE_MOTOR_ID = 2; // In radians per second
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
} // In radians per second squared
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
}
public static class ShooterConstants { //Bird
public static final int LAUNCHER_MOTOR_ID = 3; public static class ExtraIDEntities {
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
public static final int TURRET_MOTOR_ID = 4; public static final int PIGEON_ID = 1;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; }
public static final int TURRET_LIMIT_CHANNEL = 9;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 6;
public static final int HOOD_SERVO_CHANNEL = 0; public static class IntakeConstants {
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);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static class HopperConstants {
public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
public static final int SPINDEXER_MOTOR_ID = 8; public static final int DEPLOY_MOTOR_ID = 17;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
}
public static class FieldPoses { public static final int INTAKE_MOTOR_ID = 2;
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); public static class ShooterConstants {
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
} public static final int LAUNCHER_MOTOR_ID = 3;
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
public static final int TURRET_MOTOR_ID = 4;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int TURRET_LIMIT_CHANNEL = 9;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 6;
public static final int HOOD_SERVO_CHANNEL = 0;
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);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static class HopperConstants {
public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
}
public static class FieldPoses {
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
}
} }

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

@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.RelayCommand; import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand; import frc.robot.commands.SetDeflectorCommand;
import frc.robot.commands.ZoneCheck;
import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem;
@ -83,6 +84,7 @@ public class RobotContainer {
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.b().toggleOnTrue(new ZoneCheck());
} }
/** /**

4
src/main/java/frc/robot/commands/ZoneSwap.java → src/main/java/frc/robot/commands/AnyTarget.java
View File

@ -21,7 +21,7 @@ import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.TurretSubsystem;
public class ZoneSwap extends Command { public class AnyTarget extends Command {
// For Chassis Control // For Chassis Control
private DriveSubsystem driveSubsystem; private DriveSubsystem driveSubsystem;
@ -40,7 +40,7 @@ public class ZoneSwap extends Command {
private final double chassisRotateSpeed = Math.PI * 0.1; private final double chassisRotateSpeed = Math.PI * 0.1;
public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
this.driveSubsystem = driveSubsystem; this.driveSubsystem = driveSubsystem;
this.hopperSubsystem = hopperSubsystem; this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem; this.turretSubsystem = turretSubsystem;

79
src/main/java/frc/robot/commands/ZoneCheck.java Normal file
View File

@ -0,0 +1,79 @@
package frc.robot.commands;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
public class ZoneCheck extends Command {
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Zone zone;
public enum Zone {
Blue,
Red,
Neutral;
}
public ZoneCheck() {
getBotPose();
}
@Override
public void initialize() {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
switch (zone) {
case Blue:
System.out.println("Im Home");
case Neutral:
System.out.println("Uhhhhhhhh not red");
;
default:
System.out.println("BOOOOO, BOOOO RED");
;
}
} else {
switch (zone) {
case Blue:
System.out.println("Screw Blue");
;
case Neutral:
System.out.println("Uhhhhhhhh not red");
;
default:
System.out.println("Im Home");
}
}
});
}
@Override
public void execute() {
}
@Override
public boolean isFinished() {
return false;
}
private double[] getBotPose() {
return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
}
private void getZone() {
if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
zone = Zone.Red;
} else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
zone = Zone.Neutral;
} else {
zone = Zone.Blue;
}
}
}

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

@ -135,7 +135,6 @@ public class DriveSubsystem extends TestableSubsystem {
.build()) .build())
.build(); .build();
// Set vision measurement confidence values // Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
@ -166,7 +165,7 @@ public class DriveSubsystem extends TestableSubsystem {
(speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
new PPHolonomicDriveController( new PPHolonomicDriveController(
new PIDConstants(1.5, 0, 0), new PIDConstants(1.5, 0, 0),
new PIDConstants(Math.PI/2, 0, 0)), new PIDConstants(Math.PI / 2, 0, 0)),
config, config,
() -> { () -> {
var alliance = DriverStation.getAlliance(); var alliance = DriverStation.getAlliance();
@ -195,12 +194,12 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
if(frontCameraPose.tagCount >= 1){ if (frontCameraPose.tagCount >= 1) {
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
} }
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(backCameraPose.tagCount >= 1){ if (backCameraPose.tagCount >= 1) {
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
} }
@ -212,15 +211,15 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem.simulationPeriodic(); driveSystem.simulationPeriodic();
} }
public Command getAutoCommand(){ public Command getAutoCommand() {
return autoChooser.getSelected(); return autoChooser.getSelected();
} }
public void stopDrive(){ public void stopDrive() {
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
} }
public void rotateChassis(double rotSpeed){ public void rotateChassis(double rotSpeed) {
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
} }