Implement ICAT

src/main/java/frc/robot/Constants.java

@@ -7,17 +7,20 @@
 import com.ctre.phoenix.led.ColorFlowAnimation;
 import com.ctre.phoenix.led.RainbowAnimation;
 import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
+import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.frcteam3255.components.swerve.SN_SwerveConstants;
+import com.frcteam3255.preferences.SN_BooleanPreference;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants.constDrivetrain.pracBot;
 
 /*
  * @formatter:off
@@ -37,9 +40,14 @@
  * are exempt from this
  */
 public final class Constants {
-
   public static class constClimber {
-    public static final double GEAR_RATIO = 327.6;
+    public static final double GEAR_RATIO = 18.56;
+    public static final NeutralModeValue NEUTRAL_MODE = NeutralModeValue.Brake;
+    public static final GravityTypeValue GRAVITY_TYPE = GravityTypeValue.Elevator_Static;
+
+    public static final SN_BooleanPreference climberInverted = new SN_BooleanPreference("climberInverted", true);
+
+    public static final InvertedValue MOTOR_INVERTED = InvertedValue.Clockwise_Positive;
   }
 
   public static class constControllers {
@@ -83,10 +91,10 @@ public static class pracBot {
 
     // In Rotations: Obtain by aligning all of the wheels in the correct direction
     // and copy-pasting the Raw Absolute Encoder value
-    public static final double FRONT_LEFT_ABS_ENCODER_OFFSET = -0.155518;
-    public static final double FRONT_RIGHT_ABS_ENCODER_OFFSET = 0.039062;
-    public static final double BACK_LEFT_ABS_ENCODER_OFFSET = 0.399658;
-    public static final double BACK_RIGHT_ABS_ENCODER_OFFSET = -0.418945;
+    public static final double FRONT_LEFT_ABS_ENCODER_OFFSET = -0.152832;
+    public static final double FRONT_RIGHT_ABS_ENCODER_OFFSET = 0.032471;
+    public static final double BACK_LEFT_ABS_ENCODER_OFFSET = -0.107666;
+    public static final double BACK_RIGHT_ABS_ENCODER_OFFSET = 0.095215;
 
     /**
      * <p>
@@ -132,6 +140,10 @@ public static class pracBot {
   }
 
   public static class constIntake {
+    public static final double ABS_ENCODER_OFFSET = 0.102385;
+    public static final boolean ABS_ENCODER_INVERT = false;
+    public static final double GEAR_RATIO = 28.13;
+    public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake;
   }
 
   public static class constLEDs {
@@ -331,6 +343,6 @@ public static class constTransfer {
    * Locations that the robot can attempt to lock onto.
    */
   public enum LockedLocation {
-    NONE, SPEAKER
+    NONE, SPEAKER, AMP
   }
 }

src/main/java/frc/robot/Robot.java

@@ -20,6 +20,8 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+  private boolean hasAutonomousRun = false;
+
   @Override
   public void robotInit() {
     SN_Preferences.useDefaults();
@@ -69,11 +71,13 @@ public void disabledExit() {
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-    m_robotContainer.clearSubsystemMovements().schedule();
 
+    RobotContainer.zeroClimber().schedule();
+    // RobotContainer.stowIntakePivot().schedule();
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+    hasAutonomousRun = true;
   }
 
   @Override
@@ -89,7 +93,11 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.clearSubsystemMovements().schedule();
+
+    if (!hasAutonomousRun) {
+      RobotContainer.zeroClimber().schedule();
+      // RobotContainer.stowIntakePivot().schedule();
+    }
   }
 
   @Override

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

@@ -12,7 +12,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -21,11 +20,12 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Constants.constControllers;
 import frc.robot.Constants.LockedLocation;
 import frc.robot.Constants.constLEDs;
 import frc.robot.RobotMap.mapControllers;
+import frc.robot.RobotPreferences.prefClimber;
+import frc.robot.RobotPreferences.prefIntake;
 import frc.robot.RobotPreferences.prefPitch;
 import frc.robot.RobotPreferences.prefVision;
 import frc.robot.RobotPreferences.prefShooter;
@@ -41,8 +41,10 @@
 import frc.robot.commands.ManualHoodMovement;
 import frc.robot.commands.ManualTurretMovement;
 import frc.robot.commands.Panic;
+import frc.robot.commands.PrepAmp;
 import frc.robot.commands.SetLEDS;
 import frc.robot.commands.TransferGamePiece;
+import frc.robot.commands.ZeroClimber;
 import frc.robot.commands.UnaliveShooter;
 import frc.robot.commands.ZeroPitch;
 import frc.robot.commands.ZeroTurret;
@@ -115,16 +117,16 @@ public RobotContainer() {
             conDriver.btn_X,
             isPracticeBot()));
 
-    subTurret.setDefaultCommand(new LockTurret(subTurret, subDrivetrain, subClimber));
-    subPitch.setDefaultCommand(new LockPitch(subPitch, subDrivetrain, subClimber));
+    subTurret.setDefaultCommand(new LockTurret(subTurret, subDrivetrain));
+    subPitch.setDefaultCommand(new LockPitch(subPitch, subDrivetrain));
     subShooter.setDefaultCommand(new Shoot(subShooter, subLEDs));
     subLEDs
         .setDefaultCommand(new SetLEDS(subLEDs, subShooter, subTurret, subPitch, subTransfer,
             conDriver.btn_RightBumper));
 
     // Register Autonomous Named Commands
     NamedCommands.registerCommand("IntakeGamePiece",
-        new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subClimber, subPitch, subShooter));
+        new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber));
 
     // View controls at:
     // src\main\assets\controllerMap2024.png
@@ -134,6 +136,7 @@ public RobotContainer() {
 
     subDrivetrain.resetModulesToAbsolute();
     subTurret.resetTurretToAbsolutePosition();
+    subIntake.resetPivotToAbsolute();
     subLEDs.clearAnimation();
   }
 
@@ -143,50 +146,46 @@ private void configureDriverBindings(SN_XboxController controller) {
         .onTrue(
             Commands.runOnce(() -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS()[6].toPose2d())));
 
-    // // Climb Up
-
-    // controller.btn_LeftTrigger.whileTrue(
-    // Commands.run(() ->
-    // subClimber.setClimberMotorSpeed(prefClimber.climberMotorUpSpeed.getValue()),
-    // subClimber)
-    // .alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(0, false))));
-    // controller.btn_LeftTrigger.onFalse(
-    // Commands.run(() -> subClimber.setClimberMotorSpeed(0), subClimber));
-
-    // // Climb Down
-    // controller.btn_RightTrigger.whileTrue(
-    // Commands.run(() ->
-    // subClimber.setClimberMotorSpeed(prefClimber.climberMotorDownSpeed.getValue()),
-    // subClimber)
-    // .alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(0, false))));
-    // controller.btn_RightTrigger.onFalse(
-    // Commands.run(() -> subClimber.setClimberMotorSpeed(0), subClimber));
+    controller.btn_LeftTrigger
+        .whileTrue(
+            Commands.either(
+                new PrepAmp(subIntake, subPitch, subTransfer, subTurret, subShooter),
+                Commands.run(() -> subClimber.setPercentOutput(prefClimber.climberUpSpeed.getValue())),
+                () -> getLockedLocation() != LockedLocation.AMP).repeatedly())
+        .onFalse(Commands.runOnce(() -> subClimber.setPercentOutput(0)));
+
+    controller.btn_RightTrigger
+        .onTrue(Commands.runOnce(() -> subClimber.setCurrentLimiting(false)))
+        .whileTrue(Commands.run(() -> subClimber.setPercentOutput(prefClimber.climberDownSpeed.getValue())))
+        .onFalse(Commands.run(() -> subClimber.setPercentOutput(0))
+            .alongWith(Commands.runOnce(() -> subClimber.setCurrentLimiting(false))));
 
     controller.btn_RightBumper.whileTrue(Commands.run(() -> subDrivetrain.setDefenseMode(), subDrivetrain))
         .whileFalse(Commands.runOnce(() -> subLEDs.clearAnimation()));
   }
 
   private void configureOperatorBindings(SN_XboxController controller) {
     controller.btn_LeftTrigger
-        .whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subClimber, subPitch, subShooter));
+        .whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber));
     controller.btn_LeftBumper
         .onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE)))
         .whileTrue(new ManualTurretMovement(subTurret, controller.axis_RightX));
 
     controller.btn_LeftStick.whileTrue(new Panic(subLEDs));
     controller.btn_North
-        .whileTrue(new IntakeFromSource(subShooter, subTransfer, subPitch, subTurret, subClimber));
-    controller.btn_South.whileTrue(new SpitGamePiece(subIntake, subTransfer, subPitch, subClimber));
+        .whileTrue(new IntakeFromSource(subShooter, subTransfer, subPitch, subTurret));
+    controller.btn_South.whileTrue(new SpitGamePiece(subIntake, subTransfer, subPitch));
     controller.btn_West.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true)));
     controller.btn_East.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE)))
         .whileTrue(new ManualHoodMovement(subPitch, controller.axis_RightX));
 
-    controller.btn_RightTrigger.whileTrue(new TransferGamePiece(subShooter, subTurret, subTransfer, subPitch))
+    controller.btn_RightTrigger
+        .whileTrue(new TransferGamePiece(subShooter, subTurret, subTransfer, subPitch, subIntake))
         .onFalse(Commands.runOnce(() -> subTransfer.setFeederNeutralOutput())
             .alongWith(Commands.runOnce(() -> subTransfer.setTransferNeutralOutput()))
-            .alongWith(new UnaliveShooter(subShooter, subTurret, subPitch, subClimber, subLEDs)));
+            .alongWith(new UnaliveShooter(subShooter, subTurret, subPitch, subLEDs)));
 
-    controller.btn_RightBumper.onTrue(new UnaliveShooter(subShooter, subTurret, subPitch, subClimber, subLEDs)
+    controller.btn_RightBumper.onTrue(new UnaliveShooter(subShooter, subTurret, subPitch, subLEDs)
         .alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));
 
     controller.btn_Back.onTrue(new ZeroTurret(subTurret));
@@ -199,16 +198,7 @@ private void configureOperatorBindings(SN_XboxController controller) {
         .alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));
 
     // Amp Preset
-    controller.btn_B.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE))
-        .alongWith(
-            Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterAmpVelocity.getValue(),
-                prefShooter.rightShooterAmpVelocity.getValue()))
-                .alongWith(Commands.runOnce(() -> subTurret.setTurretAngle(prefTurret.turretAmpPresetPos.getValue(),
-                    subClimber.collidesWithTurret())))
-                .alongWith(Commands.runOnce(
-                    () -> subPitch.setPitchAngle(prefPitch.pitchAmpAngle.getValue(),
-                        subClimber.collidesWithPitch()))))
-        .alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false))));
+    controller.btn_B.whileTrue(new PrepAmp(subIntake, subPitch, subTransfer, subTurret, subShooter));
 
     // Subwoofer Preset
     controller.btn_X.onTrue(Commands.runOnce(() -> setLockedLocation(LockedLocation.NONE))
@@ -217,36 +207,18 @@ private void configureOperatorBindings(SN_XboxController controller) {
                 prefShooter.rightShooterSubVelocity.getValue())))
         .alongWith(
             Commands.runOnce(
-                () -> subTurret.setTurretAngle(prefTurret.turretSubPresetPos.getValue(),
-                    subClimber.collidesWithTurret())))
+                () -> subTurret.setTurretAngle(prefTurret.turretSubPresetPos.getValue())))
         .alongWith(Commands.runOnce(
-            () -> subPitch.setPitchAngle(prefPitch.pitchSubAngle.getValue(),
-                subClimber.collidesWithPitch())))
+            () -> subPitch.setPitchAngle(prefPitch.pitchSubAngle.getValue())))
         .alongWith(Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(true))));
 
-    // Trap Preset
-    // controller.btn_Y.onTrue(Commands.runOnce(() ->
-    // setLockedLocation(LockedLocation.NONE)).alongWith(
-    // Commands.runOnce(() ->
-    // subShooter.setDesiredVelocities(prefShooter.leftShooterTrapVelocity.getValue(),
-    // prefShooter.rightShooterTrapVelocity.getValue())))
-    // .alongWith(
-    // Commands.runOnce(
-    // () -> subTurret.setTurretAngle(prefTurret.turretTrapPresetPos.getValue(),
-    // subClimber.collidesWithTurret())))
-    // .alongWith(Commands.runOnce(
-    // () -> subPitch.setPitchAngle(prefPitch.pitchTrapAngle.getValue(),
-    // subClimber.collidesWithPitch()))));
-
-    controller.btn_Y.whileTrue((Commands.runOnce(() -> subClimber.setClimberVoltage(-2))));
-    controller.btn_Y.onFalse((Commands.runOnce(() -> subClimber.setClimberVoltage(0))));
-    controller.btn_Y.onFalse((Commands.runOnce(() -> subClimber.configure(true))));
-
+    // STOW
+    controller.btn_Y.onTrue(Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotStowAngle.getValue())));
   }
 
   private void configureAutoSelector() {
     autoChooser.setDefaultOption("Default Auto",
-        new DefaultAuto(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret, subClimber));
+        new DefaultAuto(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret));
   }
 
   public Command getAutonomousCommand() {
@@ -298,14 +270,6 @@ public static void logPDHValues() {
     }
   }
 
-  /**
-   * Sets all applicable subsystem's last desired location to their current
-   * location
-   */
-  public Command clearSubsystemMovements() {
-    return new InstantCommand((() -> subTurret.setTurretNeutralOutput()));
-  }
-
   // --- Locking Logic ---
 
   public static void setLockedLocation(LockedLocation location) {
@@ -331,6 +295,24 @@ public static Command zeroPitch() {
     return new ZeroPitch(subPitch).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).withTimeout(3);
   }
 
+  /**
+   * Returns the command to zero the climber. This will make the climber move
+   * itself
+   * downwards until it sees a current spike and cancel any incoming commands that
+   * require the pitch motor. If the zeroing does not end within 3 seconds, it
+   * will interrupt itself.
+   * 
+   * @return The command to zero the pitch
+   */
+  public static Command zeroClimber() {
+    return new ZeroClimber(subClimber).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
+        .withTimeout(3);
+  }
+
+  public static Command stowIntakePivot() {
+    return Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotStowAngle.getValue()));
+  }
+
   public void setAutoPlacementLEDs(Optional<Alliance> alliance) {
     startingPosition = autoChooser.getSelected().getInitialPose().get();
 

src/main/java/frc/robot/RobotMap.java

@@ -14,13 +14,6 @@ public static class mapControllers {
   // MOTORS: 30 -> 39
   public static class mapClimber {
     public static final int CLIMBER_MOTOR_CAN = 30;
-    public static final int CLIMBER_ABSOLUTE_ENCODER_DIO = 2;
-  }
-
-  // MOTORS: 50 -> 59
-  public static class mapTransfer {
-    public static final int FEEDER_MOTOR_CAN = 50;
-    public static final int TRANSFER_MOTOR_CAN = 51;
   }
 
   // MOTORS: 00 -> 09
@@ -51,7 +44,10 @@ public static class mapDrivetrain {
 
   // MOTORS: 20 -> 29
   public static class mapIntake {
-    public static final int INTAKE_ROLLER_MOTOR_CAN = 20;
+    public static final int ROLLER_CAN = 20;
+    public static final int PIVOT_CAN = 21;
+    public static final int ABSOLUTE_ENCODER_DIO = 2;
+
   }
 
   public static class mapLEDs {
@@ -75,6 +71,12 @@ public static class mapTurret {
     public static final int TURRET_ABSOLUTE_ENCODER_DIO = 1;
   }
 
+  // MOTORS: 50 -> 59
+  public static class mapTransfer {
+    public static final int FEEDER_MOTOR_CAN = 50;
+    public static final int TRANSFER_MOTOR_CAN = 51;
+  }
+
   public static class mapVision {
   }
 }