Helper functions for regions and targets
This commit is contained in:
Scott Jackson
parent
c2fa27aa5c
commit
776e3ed9d3
|
|
@ -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,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;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue