Merge branch 'main' into limelight-constants

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

@@ -230,4 +230,10 @@ public static class Climber {
     /** Whether or not the climber is inverted. */
     public static final boolean CLIMBER_INVERTED = true;
   }
+
+  /** The constants for the USB cameras */
+  public static class Camera {
+    /** The camera id for the intake camera. */
+    public static final int INTAKE_CAMERA_ID = 0;
+  }
 }

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

@@ -24,8 +24,6 @@ public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
     robotContainer = new RobotContainer();
-
-    RobotContainer.startCamera.initialize();
   }
 
   @Override

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

@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Controllers.ControllerAxis;
 import frc.robot.Controllers.Keymap;
 import frc.robot.commands.AimAndFireRoutine;
@@ -28,6 +27,7 @@
 import frc.robot.commands.EnterXMode;
 import frc.robot.commands.RunIntake;
 import frc.robot.commands.RunIntakeReverse;
+import frc.robot.commands.StartCamera;
 import frc.robot.commands.auto.MoveAwayFromAmp;
 import frc.robot.commands.auto.OnePieceAuto;
 import frc.robot.commands.auto.TaxiAuto;
@@ -94,7 +94,7 @@ public class RobotContainer {
   public static PDP pdp = new PDP();
 
   /** Singleton instance of the intake {@link Camera} for the whole robot. */
-  public static Camera intakeCamera = new Camera(0);
+  public static Camera intakeCamera = new Camera(Constants.Camera.INTAKE_CAMERA_ID);
 
   /*
    * ************
@@ -219,6 +219,9 @@ public class RobotContainer {
   /** Singleton instance of {@link ClimbDown} for the whole robot. */
   public static ClimbDown climbDown = new ClimbDown();
 
+  /** Singleton instance of the intake {@link StartCamera} for the whole robot. */
+  public static StartCamera startIntakeCamera = new StartCamera(intakeCamera);
+
   /* Autos */
   /** Singleton instance of {@link MoveAwayFromAmp} for the whole robot. */
   public static MoveAwayFromAmp moveAwayFromAmp = new MoveAwayFromAmp();
@@ -235,9 +238,6 @@ public class RobotContainer {
   /** Singleton instance of {@link ZeroGyro} for the whole robot. */
   public static ZeroGyro zeroGyro = new ZeroGyro();
 
-  public static InstantCommand startCamera =
-      new InstantCommand(intakeCamera::startCamera, intakeCamera);
-
   /*
    * ***********************
    * * OTHER INSTANCE VARS *
@@ -256,6 +256,8 @@ public RobotContainer() {
     Shuffleboard.getTab("Driver").add("Clear PDP sticky faults", clearPDPStickyFaults);
     Shuffleboard.getTab("Driver").add("Zero Gyro", zeroGyro);
 
+    startIntakeCamera.schedule();
+
     switch (Constants.Drive.currentDrivePerspective) {
       case RobotOriented:
         drive.setDefaultCommand(driveRobotOriented);

src/main/java/frc/robot/commands/StartCamera.java

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Camera;
+
+/** Command that starts up a camera and sends it to shuffleboard. */
+public class StartCamera extends Command {
+  private Camera camera;
+
+  /** Creates a new StartCamera. */
+  public StartCamera(Camera camera) {
+    this.camera = camera;
+    addRequirements(camera);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    camera.startCamera();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}