From 9f7957f874d98e8cb59eb63a6bbabbc4e5303254 Mon Sep 17 00:00:00 2001 From: David Shen Date: Fri, 11 Apr 2025 21:21:32 -0400 Subject: [PATCH 1/2] l1 load, wheels start at distance --- .../frc/robot/constants/AlgaeConstants.java | 2 +- .../frc/robot/constants/RobotConstants.java | 4 ++-- .../robot/constants/RobotStateConstants.java | 3 +++ .../robotState/RobotStateSubsystem.java | 18 +++++++++++++++++- 4 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 24d3ac74..610eec1f 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 1fafff00..7c40be02 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 0d872176..7d5ab38e 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 429b6168..fc07fb80 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; @@ -455,13 +456,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); @@ -1105,6 +1107,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(); From 4f93ef00b0360910ddd2dc3a8613e8aa86d64b03 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sat, 12 Apr 2025 13:09:05 -0400 Subject: [PATCH 2/2] remove experimental wallEYE --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 67c46df9..8fa4c073 100644 --- a/build.gradle +++ b/build.gradle @@ -81,7 +81,7 @@ dependencies { implementation 'ch.qos.logback:logback-classic:1.3.5' //logging implementation("com.opencsv:opencsv:5.6") - implementation 'org.strykeforce:wallEYE:25.1.1' + implementation 'org.strykeforce:wallEYE:25.1.0' implementation 'net.jafama:jafama:2.3.2' //fastMath }