New Algae End Effector, Gyro Fixes

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.0.1'
+    implementation 'org.strykeforce:wallEYE:25.1.0'
     implementation 'net.jafama:jafama:2.3.2' //fastMath
 }
 

src/main/java/frc/robot/RobotContainer.java

@@ -28,6 +28,7 @@
 import frc.robot.commands.biscuit.HoldBiscuitCommand;
 import frc.robot.commands.biscuit.JogBiscuitCommand;
 import frc.robot.commands.biscuit.ZeroBiscuitCommand;
+import frc.robot.commands.climb.AutoClimbCommand;
 import frc.robot.commands.climb.ClimbCommand;
 import frc.robot.commands.climb.ClimbPrepCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
@@ -70,6 +71,7 @@
 import frc.robot.subsystems.battMon.BattMonSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitIOFXS;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.climb.ClimbAlignSubsystem;
 import frc.robot.subsystems.climb.ClimbIO;
 import frc.robot.subsystems.climb.ClimbIOServoFX;
 import frc.robot.subsystems.climb.ClimbSubsystem;
@@ -109,6 +111,7 @@ public class RobotContainer {
 
   private final ClimbIO climbIO;
   private final ClimbSubsystem climbSubsystem;
+  private final ClimbAlignSubsystem climbAlignSubsystem;
 
   private final CoralIOFX coralIO;
   private final CoralSubsystem coralSubsystem;
@@ -170,6 +173,7 @@ public RobotContainer() {
       protoSwerve = new Swerve();
       driveSubsystem = new DriveSubsystem(protoSwerve);
     }
+    climbAlignSubsystem = new ClimbAlignSubsystem(climbSubsystem, driveSubsystem);
 
     elevatorIO = new ElevatorIOFX();
     elevatorSubsystem = new ElevatorSubsystem(elevatorIO);
@@ -190,6 +194,7 @@ public RobotContainer() {
             battMonSubsystem,
             biscuitSubsystem,
             climbSubsystem,
+            climbAlignSubsystem,
             coralSubsystem,
             driveSubsystem,
             elevatorSubsystem,
@@ -658,6 +663,11 @@ public void configTestDash() {
         .withPosition(1, 0)
         .withSize(1, 1);
 
+    Shuffleboard.getTab("Test")
+        .addDouble("Yaw Camera Idx", () -> visionSubsystem.getYawUpdateCamera())
+        .withPosition(1, 1)
+        .withSize(1, 1);
+
     // Shuffleboard.getTab("Test")
     //     .add("Start Auton", nonProcessorShallowAutonCommand)
     //     .withPosition(3, 0)
@@ -685,6 +695,14 @@ public void configTestDash() {
         .add("Set isAuto True", new InstantCommand(() -> robotStateSubsystem.setIsAuto(true)))
         .withPosition(7, 0)
         .withSize(1, 1);
+
+    Shuffleboard.getTab("Test")
+        .add(
+            "Auto Climb",
+            new AutoClimbCommand(
+                driveSubsystem, climbSubsystem, climbAlignSubsystem, robotStateSubsystem))
+        .withPosition(0, 1)
+        .withSize(1, 1);
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/climb/AutoClimbCommand.java

@@ -0,0 +1,43 @@
+package frc.robot.commands.climb;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climb.ClimbAlignSubsystem;
+import frc.robot.subsystems.climb.ClimbAlignSubsystem.ClimbAlignStates;
+import frc.robot.subsystems.climb.ClimbSubsystem;
+import frc.robot.subsystems.climb.ClimbSubsystem.ClimbState;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+
+public class AutoClimbCommand extends Command {
+  private ClimbSubsystem climbSubsystem;
+  private ClimbAlignSubsystem climbAlignSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+
+  public AutoClimbCommand(
+      DriveSubsystem driveSubsystem,
+      ClimbSubsystem climbSubsystem,
+      ClimbAlignSubsystem climbAlignSubsystem,
+      RobotStateSubsystem robotStateSubsystem) {
+    this.climbAlignSubsystem = climbAlignSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.climbSubsystem = climbSubsystem;
+
+    addRequirements(climbSubsystem, driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    climbAlignSubsystem.start(robotStateSubsystem.getAllianceColor());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    climbAlignSubsystem.terminate();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return climbAlignSubsystem.getState() == ClimbAlignStates.DONE
+        && climbSubsystem.getState() == ClimbState.CLIMBED;
+  }
+}

src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java

@@ -44,6 +44,7 @@ public void initialize() {
   @Override
   public void end(boolean interrupted) {
     tagAlignSubsystem.terminate();
+    driveSubsystem.setIgnoreSticks(false);
   }
 
   @Override

src/main/java/frc/robot/constants/BiscuitConstants.java

@@ -46,8 +46,8 @@ public class BiscuitConstants {
   // public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089
 
   // jogging
-  public static final double kJogAmountUp = 10;
-  public static final double kJogAmountDown = -10;
+  public static final double kJogAmountUp = 3;
+  public static final double kJogAmountDown = -3;
 
   // Soft Limits
   // public static final Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5));

src/main/java/frc/robot/constants/ClimbConstants.java

@@ -43,9 +43,24 @@ public class ClimbConstants {
 
   // Climb positions
   // public static final Angle kClimbCagePos = Rotations.of(); // will test
-  public static final Double kClimbRatchedEngage = -0.12;
+  public static final double kClimbRatchedEngage = -0.12;
   public static final double kFullyClimbed = kArmMaxFwd;
-  public static final double kClimbOpenLoopSpeed = 4.0;
+  public static final double kClimbOpenLoopSpeed = 3.0;
+
+  // Cage location info
+  public static final double kCenterX = DriveConstants.kFieldMaxX / 2.0;
+  public static final double kCenterY = DriveConstants.kFieldMaxY / 2.0;
+  public static final double[] kCageOffsetY = {1.0541, 2.143125, 3.235325};
+  public static final double kCageOffsetX = 1.2;
+
+  public static final double kClimbRobotOffset = 0.0075;
+
+  public static final double kFinalDriveVx = 0.25;
+  public static final double kMaxDistX = 3;
+  // Align end conditions
+  public static final double kCloseEnoughX = 0.1;
+  public static final double kCloseEnoughY = 0.03;
+  public static final double kClimbAngleGood = RobotStateConstants.kClimbAngleSmall;
 
   public static TalonFXConfiguration getPivotArmFxConfig() {
     TalonFXConfiguration armFxConfig = new TalonFXConfiguration();

src/main/java/frc/robot/constants/DriveConstants.java

@@ -45,7 +45,7 @@ public class DriveConstants {
   public static final double kMaxSpeedMetersPerSecond = 3.782;
   public static final double kSpeedStillThreshold = 0.1; // meters per second
   public static final double kGyroRateStillThreshold = 10.0; // 25  5 degrees per second
-  public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
+  public static final double kGyroDifferentThreshold = 0.5; // 5 degrees
   public static final int kGyroDifferentCount = 3;
 
   public static final double kRobotLength = 0.61595;

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -43,8 +43,8 @@ public class ElevatorConstants {
   // Idle
 
   // Algae removal
-  public static final Angle kL2AlgaeSetpoint = Rotations.of(5.826); // was 6.308
-  public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513);
+  public static final Angle kL2AlgaeSetpoint = Rotations.of(6.3457); // was 6.308
+  public static final Angle kL3AlgaeSetpoint = Rotations.of(17.428223);
 
   public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint;
   public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
@@ -60,13 +60,13 @@ public class ElevatorConstants {
   public static final Angle kAutoPrestageSetpoint = kL4CoralSetpoint;
 
   // Algae obtaining
-  public static final Angle kFloorAlgaeSetpoint = Rotations.of(6.66);
-  public static final Angle kMicAlgaeSetpoint = Rotations.of(17.07);
+  public static final Angle kFloorAlgaeSetpoint = Rotations.of(5.82);
+  public static final Angle kMicAlgaeSetpoint = Rotations.of(2.703);
   public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063);
 
   // Algae scoring
-  public static final Angle kProcessorSetpoint = Rotations.of(6.3215); // was 4.297 -> 5.964
-  public static final Angle kBargeSetpoint = Rotations.of(44.943); // 41.936
+  public static final Angle kProcessorSetpoint = Rotations.of(3.348); // was 4.297 -> 5.964
+  public static final Angle kBargeSetpoint = Rotations.of(44.785); // 41.936
   public static final Angle kBargeHigherThan = Rotations.of(31.0901);
 
   public static TalonFXConfiguration getBothFXConfig() {

src/main/java/frc/robot/constants/RobotConstants.java

@@ -79,7 +79,7 @@ public class RobotConstants {
   // Algae scoring
   public static Angle kProcessorSetpoint;
   public static Angle kBargeSetpoint;
-  public static Angle kBargeBackwardSetpoint;
+  // public static Angle kBargeBackwardSetpoint;
 
   public static double kTagAlignThreshold;
 
@@ -108,7 +108,7 @@ public RobotConstants() {
       kStowSetpoint = CompConstants.kBiscuitStowSetpoint;
       kFunnelSetpoint = CompConstants.kFunnelSetpoint;
       kPrestageSetpoint = CompConstants.kPrestageSetpoint;
-      kPrestageAlgaeSetpoint = CompConstants.kPrestageAlgaeSetpoint;
+      kPrestageAlgaeSetpoint = CompConstants.kBiscuitStowSetpoint;
 
       // Algae removal
       kL2AlgaeSetpoint = CompConstants.kL2AlgaeSetpoint;
@@ -131,7 +131,7 @@ public RobotConstants() {
       // Algae scoring
       kProcessorSetpoint = CompConstants.kProcessorSetpoint;
       kBargeSetpoint = CompConstants.kBargeSetpoint;
-      kBargeBackwardSetpoint = CompConstants.kBargeBackwardSetpoint;
+      // kBargeBackwardSetpoint = CompConstants.kBargeBackwardSetpoint;
       kTagAlignThreshold = CompConstants.kTagAlignThreshold;
     } else {
       // Proto constants
@@ -177,7 +177,7 @@ public RobotConstants() {
       // Algae scoring
       kProcessorSetpoint = ProtoConstants.kProcessorSetpoint;
       kBargeSetpoint = ProtoConstants.kBargeSetpoint;
-      kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint;
+      // kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint;
 
       kTagAlignThreshold = ProtoConstants.kTagAlignThreshold;
     }
@@ -229,13 +229,13 @@ public static class ProtoConstants {
     // Algae scoring
     public static Angle kProcessorSetpoint = Rotations.of(41.193);
     public static Angle kBargeSetpoint = Rotations.of(12.3489);
-    public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089
+    // public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089
 
     // Elevator
     public static Angle kElevatorFunnelSetpoint = Rotations.of(2.03125);
     public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint;
     public static Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5));
-    public static Angle kMaxRev = kBargeBackwardSetpoint.minus(Rotations.of(5));
+    public static Angle kMaxRev = kPrestageSetpoint.minus(Rotations.of(5));
 
     public static MotionMagicConfigs getAlgaeMotionConfig() {
       MotionMagicConfigs algaeConfig =
@@ -343,36 +343,36 @@ public static class CompConstants {
     // Idle
     public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2);
     public static Angle kFunnelSetpoint = kBiscuitStowSetpoint;
-    public static Angle kPrestageSetpoint = kBiscuitStowSetpoint;
-    public static Angle kPrestageAlgaeSetpoint = Rotations.of(9.089 / 2);
+    public static Angle kPrestageSetpoint = Rotations.of(-2.94);
+    public static Angle kPrestageAlgaeSetpoint = kBiscuitStowSetpoint;
 
     // Algae removal
-    public static Angle kL2AlgaeSetpoint = Rotations.of(24.104 / 2);
-    public static Angle kL3AlgaeSetpoint = Rotations.of(24.562 / 2);
+    public static Angle kL2AlgaeSetpoint = Rotations.of(7.39);
+    public static Angle kL3AlgaeSetpoint = Rotations.of(7.39);
 
-    public static Angle kL2AlgaeRemovalSetpoint = kBiscuitStowSetpoint;
-    public static Angle kL3AlgaeRemovalSetpoint = kBiscuitStowSetpoint;
+    public static Angle kL2AlgaeRemovalSetpoint = kPrestageSetpoint;
+    public static Angle kL3AlgaeRemovalSetpoint = kPrestageSetpoint;
 
     public static double kTagAlignThreshold = 20.0 / 2;
 
     // Coral score
-    public static Angle kL1CoralSetpoint = kBiscuitStowSetpoint;
-    public static Angle kL2CoralSetpoint = kBiscuitStowSetpoint;
-    public static Angle kL3CoralSetpoint = kBiscuitStowSetpoint;
-    public static Angle kL4CoralSetpoint = kBiscuitStowSetpoint;
+    public static Angle kL1CoralSetpoint = kPrestageSetpoint;
+    public static Angle kL2CoralSetpoint = kPrestageSetpoint;
+    public static Angle kL3CoralSetpoint = kPrestageSetpoint;
+    public static Angle kL4CoralSetpoint = kPrestageSetpoint;
 
     // Algae obtaining
-    public static Angle kFloorAlgaeSetpoint = Rotations.of(49.627 / 2);
-    public static Angle kMicAlgaeSetpoint = Rotations.of(51.61872 / 2);
-    public static Angle kHpAlgaeSetpoint = Rotations.of(16.97559 / 2);
+    public static Angle kFloorAlgaeSetpoint = Rotations.of(20.039);
+    public static Angle kMicAlgaeSetpoint = Rotations.of(12.87085);
+    public static Angle kHpAlgaeSetpoint = kBiscuitStowSetpoint;
 
     // Algae scoring
-    public static Angle kProcessorSetpoint = Rotations.of(41.193 / 2);
-    public static Angle kBargeSetpoint = Rotations.of(12.3489 / 2);
-    public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489 / 2); // 9.089
+    public static Angle kProcessorSetpoint = Rotations.of(15.0139);
+    public static Angle kBargeSetpoint = Rotations.of(2.697);
+    // public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489 / 2); // 9.089
 
-    public static Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5 / 2));
-    public static Angle kMaxRev = kBargeBackwardSetpoint.minus(Rotations.of(5 / 2));
+    public static Angle kMaxFwd = kFloorAlgaeSetpoint.plus(Rotations.of(5));
+    public static Angle kMaxRev = kPrestageSetpoint.minus(Rotations.of(5));
 
     public static MotionMagicConfigs getAlgaeMotionConfig() {
       MotionMagicConfigs algaeConfig =

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -7,7 +7,7 @@ public class RobotStateConstants {
   public static final double kBlueBargeSafeX = 7.6;
   public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
-  public static final double kCoralEjectTimer = 0.75;
+  public static final double kCoralEjectTimer = 0.25;
   public static final double kAlgaeEjectTimer = 0.5;
 
   public static final double kProcessorStowRadius = 0.5;

src/main/java/frc/robot/constants/TagServoingConstants.java

@@ -51,8 +51,8 @@ public class TagServoingConstants {
   public static final double kCoralAlignRadius = 1.32; // 1.293823 is perfectly against the reef
   // public static final double kCoralStopXDriveRadius =
   //     kCoralInitialDriveRadius; // Should be closer to reef than target pose
-  public static final double kAlgaeInitialDriveRadius = 1.75;
-  public static final double kAlgaeAlignRadius = 1.34; // was 1.34
+  public static final double kAlgaeInitialDriveRadius = kCoralInitialDriveRadius; // 1.75;
+  public static final double kAlgaeAlignRadius = kCoralAlignRadius; // 1.34; // was 1.34
   public static final double kAlgaeStopXDriveRadius =
       kAlgaeInitialDriveRadius; // Should be closer to reef than target pose
   // public static final double kMinVelX = 0.85;