Central Valley Regional fixes (#341)

* Static hook lock on init

* Added lift intake to prep climb

Co-authored-by: Driverstation 2 <[email protected]>

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

@@ -40,9 +40,9 @@ public void robotInit() {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
     DriverStation.silenceJoystickConnectionWarning(true);
-    PortForwarder.add(5800, "limelight.local", 5800);
-    PortForwarder.add(5801, "limelight.local", 5801);
-    PortForwarder.add(5805, "limelight.local", 5805);
+    PortForwarder.add(5800, "10.32.55.11", 5800);
+    PortForwarder.add(5801, "10.32.55.11", 5801);
+    PortForwarder.add(5805, "10.32.55.11", 5805);
 
     // Intake camera
     CameraServer.startAutomaticCapture();
@@ -95,6 +95,7 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    m_robotContainer.sub_climber.hookDown();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
@@ -113,6 +114,7 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+    m_robotContainer.sub_climber.hookDown();
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

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

@@ -54,7 +54,7 @@ public class RobotContainer {
   private final Turret sub_turret = new Turret();
   private final Intake sub_intake = new Intake();
   private final Shooter sub_shooter = new Shooter();
-  private final Climber sub_climber = new Climber();
+  public final Climber sub_climber = new Climber();
   private final Transfer sub_transfer = new Transfer();
   private final NavX sub_navX = new NavX();
   private final Vision sub_vision = new Vision();
@@ -161,7 +161,7 @@ public class RobotContainer {
   private final InstantCommand com_hookClimberDown = new InstantCommand(sub_climber::hookDown);
   private final RunSpool com_runSpool = new RunSpool(sub_climber);
 
-  private final PrepClimb com_prepClimb = new PrepClimb(sub_turret, sub_hood, sub_climber);
+  private final PrepClimb com_prepClimb = new PrepClimb(sub_turret, sub_hood, sub_climber, sub_intake);
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.

src/main/java/frc/robot/RobotPreferences.java

@@ -227,7 +227,7 @@ public static final class VisionPrefs {
   public static final class ClimberPrefs {
     public static final SN_DoublePreference climberMotorSpeed = new SN_DoublePreference("climberMotorSpeed", 0.5);
     public static final SN_DoublePreference climberMaxEncoderCountPerpendicular = new SN_DoublePreference(
-        "climberMaxEncoderCount", 284000);
+        "climberMaxEncoderCount", 240000);
     public static final SN_DoublePreference climberMaxEncoderCountAngled = new SN_DoublePreference(
         "climberMaxEncoderCountPivoted", 240000);
 

src/main/java/frc/robot/commands/Climber/PrepClimb.java

@@ -11,23 +11,26 @@
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Hood;
 import frc.robot.subsystems.Turret;
+import frc.robot.subsystems.Intake;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class PrepClimb extends SequentialCommandGroup {
   /** Creates a new PrepClimb. */
-  public PrepClimb(Turret sub_turret, Hood sub_hood, Climber sub_climber) {
+  public PrepClimb(Turret sub_turret, Hood sub_hood, Climber sub_climber, Intake sub_intake) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
         // Zero Turret
-        new SetTurretPosition(sub_turret, RobotPreferences.TurretPrefs.turretSnapAwayIntake),
+        new SetTurretPosition(sub_turret, RobotPreferences.TurretPrefs.turretSnapAwayIntake).withTimeout(0.5),
         // Retract hood
         new InstantCommand(sub_hood::hoodZeroTilt, sub_hood),
+        // Lift Intake
+        new InstantCommand(sub_intake::retractIntake),
         // Deploy stationary hooks
         new InstantCommand(sub_climber::hookUp, sub_climber),
         // Angle climber
-        new InstantCommand(sub_climber::pivotAngled, sub_climber));
+        new InstantCommand(sub_climber::pivotPerpendicular, sub_climber));
   }
 }