Merge pull request #72 from strykeforce/l1-positions

Level 1 - New End Effector

Author: mwitcpalek

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

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;
 

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 = {

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();