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

Merge branch 'main' of github.com:FairportRobotics/Castor2026

This commit is contained in:
TheRealGrayden 2026-04-13 19:57:32 -04:00
parent 9c85cd1c8a d47273c50d
commit 2fdfa62816
4 changed files with 198 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto
View File

@ -35,6 +35,25 @@
"pathName": "1.2 Shoot to Depot Path" "pathName": "1.2 Shoot to Depot Path"
} }
}, },
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "RunIntake"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {

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

@ -9,6 +9,7 @@ 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.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
@ -170,5 +171,50 @@ public final class Constants {
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); 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))); public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
// These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
// Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
// Rotation are zero because they don't matter.
public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
}
// These represent the dividing lines for the regions in our shooting strategy.
public static class ShootingRegionDimensions {
public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
public static double RED_LEFT_REGION_Y_MAX = 3.0; // same meaning as the previous one, but from red perspective
public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
public static double RED_RIGHT_REGION_Y_MIN = 5.0; // same meaning as the previous one, but from red perspective
}
// Regions that define the AutoTurret shooting behaviors.
// Color ID in the name reflects robot's alliance since region definitions vary by alliance.
// Left/Right are from driver's perspective.
public enum ShootingRegion {
NON_SHOOTING_REGION, // Any part of the field that's not in one of the regions
BLUE_OWN_ALLIANCE_ZONE,
BLUE_LEFT_NEUTRAL_ZONE,
BLUE_RIGHT_NEUTRAL_ZONE,
BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
RED_OWN_ALLIANCE_ZONE,
RED_LEFT_NEUTRAL_ZONE,
RED_RIGHT_NEUTRAL_ZONE,
RED_LEFT_OPPONENT_ALLIANCE_ZONE,
RED_RIGHT_OPPONENT_ALLIANCE_ZONE
} }
} }

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

@ -53,6 +53,7 @@ public class RobotContainer {
configureBindings(); configureBindings();
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake());
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
} }

132
src/main/java/frc/robot/Utils.java
View File

@ -1,5 +1,11 @@
package frc.robot; package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Constants.ShootingRegionDimensions;
import edu.wpi.first.math.geometry.Pose3d;
public class Utils { public class Utils {
/** /**
@ -22,4 +28,130 @@ public class Utils {
return robotTurretRotations; return robotTurretRotations;
} }
/**
* Determines the region for the robot based on its location and alliance.
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
* @param robotPose: the robot's pose
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the robot's shooting region
*/
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region
double robotX = robotPose.getX();
double robotY = robotPose.getY();
// Receommended: Add a sanity check for invalid robot poses.
if (alliance == Alliance.Blue) {
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
else if (alliance == Alliance.Red) {
if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
}
else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
return region;
}
/**
* Determines the appropriate shooting target for the robot based on its alliance
* and its current region on the field. There are 10 possible targets:
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
* and these exist for both red and blue alliances.
* Right/left are from driver's perspective.
* @param region: the identifier of the robot's current field region
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the shooting target for a robot with the provided properties. Returns all-zero
* Pose3d object if the region/alliance combination is invalid.
*/
public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
Pose3d shootingTarget = new Pose3d();
if (alliance == Alliance.Blue) {
switch (region) {
case BLUE_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
break;
case BLUE_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
else if (alliance == Alliance.Red) {
switch (region) {
case RED_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
break;
case RED_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
return shootingTarget;
}
} }