diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index e20119bb..431ac626 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -44,37 +44,52 @@ public Superstructure(Shooter shooter) { this.shooter = shooter; if(CURRENT_MODE == Mode.SIM) { shouldStowTrigger.whileTrue( - shooter.stowCommand()); + shooter.stowCommand() + ); + scoringModeTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( HUB, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryLeftTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( FERRY_LEFT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryRightTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( FERRY_RIGHT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + } else { shouldStowTrigger.whileTrue( - shooter.stowCommand()); + shooter.stowCommand() + ); + scoringModeTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(HUB, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryLeftTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(FERRY_LEFT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryRightTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(FERRY_RIGHT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); } } @@ -143,7 +158,7 @@ public void resetTrenchZones() { public void updateTrenchZonesVeloBased() { //Updates width of zone based on robot velocity Distance dynamicTrenchDangerZoneWidth = Meters.of(TRENCH_ZONE_OFFSET.in(Meters) + Math.abs(RobotContainer.drivetrain.getFieldRelativeVelocity().getX())* HoodConstants.HOOD_LOWER_TIME); - FieldConstants.Zones.TRENCH_DANGER_ZONES = new Translation2d[][]{ + FieldConstants.Zones.TRENCH_DANGER_ZONES = new Translation2d[][] { // near right trench new Translation2d[] { new Translation2d(LinesVertical.HUB_CENTER.minus(dynamicTrenchDangerZoneWidth), Meters.zero()), @@ -180,7 +195,7 @@ public static boolean isInScoringZone() { return (AllianceManager.isRed() ? pos.getX() >= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.HUB_CENTER.in(Meters), 0)).getX() - : + : pos.getX() <= FieldConstants.LinesVertical.HUB_CENTER.in(Meters) ); } @@ -189,52 +204,42 @@ public boolean shouldBeScoringBasedOnZones() { //for now // if(!AllianceManager.isAllianceKnown() || (HubTracker.getCurrentShift().isEmpty())) return false; - if(!AllianceManager.isAllianceKnown()) return false; - - return !inTrenchDangerZone() && isInScoringZone(); //Are we in the scoring zone and is the hub active // return // !inTrenchDangerZone() // && isInScoringZone() // && HubTracker.isActive(DriverStation.getAlliance().get(), HubTracker.getCurrentShift().get()); + + return AllianceManager.isAllianceKnown() ? !inTrenchDangerZone() && isInScoringZone() : false; } public boolean shouldFerryLeft() { - if(!AllianceManager.isAllianceKnown()) return false; - Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); + Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); - return - !inTrenchDangerZone() && - ( - AllianceManager.isRed() ? + return !AllianceManager.isAllianceKnown() ? false : !inTrenchDangerZone() && (AllianceManager.isRed() ? ( pos.getY() <= PoseUtils.flipTranslationAlliance(new Translation2d(0, FieldConstants.LinesHorizontal.CENTER.in(Meters))).getY() && pos.getX() <= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters), 0)).getX() ) : ( - pos.getY() >= FieldConstants.LinesHorizontal.CENTER.in(Meters) - && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) + pos.getY() >= FieldConstants.LinesHorizontal.CENTER.in(Meters) && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) ) ); } public boolean shouldFerryRight() { - if(!AllianceManager.isAllianceKnown()) return false; - Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); + Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); - return - !inTrenchDangerZone() && - (AllianceManager.isRed() ? - ( + return !AllianceManager.isAllianceKnown() ? false : !inTrenchDangerZone() && (AllianceManager.isRed() ? + ( pos.getY() >= PoseUtils.flipTranslationAlliance(new Translation2d(0, FieldConstants.LinesHorizontal.CENTER.in(Meters))).getY() && pos.getX() <= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters), 0)).getX() ) - : + : ( - pos.getY() <= FieldConstants.LinesHorizontal.CENTER.in(Meters) - && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) + pos.getY() <= FieldConstants.LinesHorizontal.CENTER.in(Meters) && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) ) ); } @@ -248,9 +253,11 @@ public static boolean inTrenchZone() { && robotPose.getX() <= zone[1].getX() && robotPose.getY() >= zone[0].getY() && robotPose.getY() <= zone[1].getY()) { + return true; } } + return false; } @@ -262,9 +269,11 @@ public static boolean inTrenchDangerZone() { && turretPose.getX() <= zone[1].getX() && turretPose.getY() >= zone[0].getY() && turretPose.getY() <= zone[1].getY()) { + return true; } } + return false; } @@ -280,18 +289,22 @@ public static boolean inBumpZone() { && robotPose.getX() <= zone[1].getX() && robotPose.getY() >= zone[0].getY() && robotPose.getY() <= zone[1].getY()) { + return true; } } + return false; } // are we moving INTO the trench? public static boolean movingIntoObstacle() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); + boolean movingIntoObstacleOnBlue = (robotPose.getX() < FieldConstants.LinesVertical.ALLIANCE_ZONE.plus(FieldConstants.Hub.WIDTH.div(2)).plus(Constants.BUMPER_WIDTH).in(Meters) - && RobotContainer.drivetrain.getForwardVelocityFromController() > 0 ) + + && RobotContainer.drivetrain.getForwardVelocityFromController() > 0) || (robotPose.getX() > FieldConstants.LinesVertical.ALLIANCE_ZONE.in(Meters) && robotPose.getX() < FieldConstants.LinesVertical.CENTER.in(Meters) @@ -304,7 +317,6 @@ public static boolean movingIntoObstacle() { || (robotPose.getX() > FieldConstants.LinesVertical.OPP_ALLIANCE_ZONE.minus(FieldConstants.Hub.WIDTH.div(2)).minus(Constants.BUMPER_WIDTH).in(Meters) && RobotContainer.drivetrain.getForwardVelocityFromController() < 0); - if(AllianceManager.isRed()) return !movingIntoObstacleOnBlue; - return movingIntoObstacleOnBlue; + return AllianceManager.isRed() ? !movingIntoObstacleOnBlue : movingIntoObstacleOnBlue; } }