diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 24d3ac7..610eec1 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -32,7 +32,7 @@ public class AlgaeConstants { public static final double kProcessorScoreSpeed = -1; public static final double kCoralScoreSpeed = 0.5; public static final double kIntakingSpeed = 1; // 0.75 - public static final double kCoralIntakingSpeed = -0.75; + public static final double kCoralIntakingSpeed = -0.3; // -0.75; public static final double kHasAlgaeVelThreshold = 70; // 10 public static final double kSuperCycleHasAlgaeVelThres = 40; // was 40 diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 1fafff0..7c40be0 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -349,7 +349,7 @@ public static TalonFXSConfiguration getFXSConfig() { public static class CompConstants { public static Angle kElevatorFunnelSetpoint = Rotations.of(0.3676757); - public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.84); + public static Angle kElevatorL1LoadSetpoint = Rotations.of(0.63); public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint; // Biscuit @@ -366,7 +366,7 @@ public static class CompConstants { // Idle public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2); public static Angle kFunnelSetpoint = kBiscuitStowSetpoint; - public static Angle kL1CoralLoadSetpoint = Rotations.of(8.078); + public static Angle kL1CoralLoadSetpoint = Rotations.of(6.29); // 0.05 is about an inch public static Angle kPrestageSetpoint = Rotations.of(-2.94); public static Angle kPrestageAlgaeSetpoint = kBiscuitStowSetpoint; diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 0d87217..7d5ab38 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -30,6 +30,9 @@ public class RobotStateConstants { public static final double kClimbAngleSmall = -0.235; // -0.245 public static final double kClimbAngleBig = -0.213; // -0.215 + // Funnel load algae on + public static final double kL1FunnelLoadX = 2.5; + // Offsets public static final Angle kStuckCoralElevatorOffset = Rotations.of(3.746); public static final Angle[][][] kBlueCoralElevatorOffset = { diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 4f35a7e..e5892ed 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -15,6 +15,7 @@ import frc.robot.constants.RobotStateConstants; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates; import frc.robot.subsystems.battMon.BattMonSubsystem; import frc.robot.subsystems.biscuit.BiscuitSubsystem; import frc.robot.subsystems.climb.ClimbAlignSubsystem; @@ -463,13 +464,14 @@ public void toFunnelLoad() { if (scoringLevel == ScoringLevel.L1) { elevatorSubsystem.setPosition(RobotConstants.kElevatorL1LoadSetpoint); - algaeSubsystem.intakeCoral(); + funnelSubsystem.reverse(); coralSubsystem.stop(); setState(RobotStates.TO_ALGAE_CORAL_LOAD, false); } else { setBiscuitTransfer(RobotConstants.kFunnelSetpoint, true); + algaeSubsystem.holdAlgae(); elevatorSubsystem.setPosition(RobotConstants.kElevatorFunnelSetpoint); coralSubsystem.intake(); setState(RobotStates.FUNNEL_LOAD, true); @@ -1116,6 +1118,20 @@ public void periodic() { if (scoringLevel != ScoringLevel.L1) { toFunnelLoad(); } + + double currentX = driveSubsystem.getPoseMeters().getX(); + + if (allianceColor == Alliance.Blue && currentX < RobotStateConstants.kL1FunnelLoadX + || allianceColor == Alliance.Red + && currentX > (DriveConstants.kFieldMaxX - RobotStateConstants.kL1FunnelLoadX)) { + if (algaeSubsystem.getState() != AlgaeStates.CORAL_INTAKE + && algaeSubsystem.getState() != AlgaeStates.HAS_CORAL) { + algaeSubsystem.intakeCoral(); + } + } else if (algaeSubsystem.getState() == AlgaeStates.CORAL_INTAKE) { + algaeSubsystem.holdAlgae(); + } + if (algaeSubsystem.hasCoral()) { coralLoc = CoralLoc.ALGAE; toPrestage();