Merge pull request #142 from frc937/heartland-2024

All Changes from Heartland 2024 MEGAMERGE PAAAAAAAIIIIIIIINNNNNN
frc937 · Mar 19, 2024 · 59bf5eb · 59bf5eb
2 parents 122f17a + 813bf3a
commit 59bf5eb
Show file tree

Hide file tree

Showing 10 changed files with 129 additions and 29 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -12,6 +12,7 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
+import com.revrobotics.CANSparkBase.IdleMode;
 import edu.wpi.first.math.geometry.Translation2d;
 
 /**
@@ -31,7 +32,7 @@ public static class Mailbox {
     public static final int MAILBOX_LIMIT_SWITCH_DIO_PORT = 0;
 
     /** The delay between raising the mailbox and firing the note. In seconds. */
-    public static final double FIRE_NOTE_DELAY_TIME = 0.25;
+    public static final double FIRE_NOTE_DELAY_TIME = 1.25;
   }
 
   /** Constants for the Pneumatics system. */
@@ -84,23 +85,33 @@ public static class Controllers {
 
   /** Constants for the Drivetrain */
   public static class Drive {
+
+    /* TODO: MAKE THIS TOGGLEABLE BY DRIVERS - PROBABLY POST-HEARTLAND */
+    public static enum DrivePerspectiveOptions {
+      RobotOriented,
+      FieldOriented
+    }
+
+    public static DrivePerspectiveOptions currentDrivePerspective =
+        DrivePerspectiveOptions.FieldOriented;
+
     /** Empty translation to prevent creating 2 Translation2ds every time the drive train stops. */
     public static Translation2d EMPTY_TRANSLATION = new Translation2d();
 
     /** The max speed the robot can go in m/s */
-    public static double MAX_SPEED = 2;
+    public static double MAX_SPEED = 100;
 
     /** The max speed the robot can rotate */
-    public static double MAX_ANGULAR_SPEED = Math.PI;
+    public static double MAX_ANGULAR_SPEED = 100;
 
     /** The distance from the center of the robot to any of the swerve modules. */
     public static double DISTANCE_ROBOT_CENTER_TO_SWERVE_MODULE = 0.3;
 
     /** The max speed for the robot when not sprinting (in m/s) */
-    public static final double MAX_NORMAL_SPEED = 1;
+    public static final double MAX_NORMAL_SPEED = 2;
 
     /** The max angular speed (in randians) for the robot when not sprinting */
-    public static final double MAX_NORMAL_ANGULAR_SPEED = Math.PI;
+    public static final double MAX_NORMAL_ANGULAR_SPEED = 2 * Math.PI;
 
     /** The Translation Drive PID for the robot. */
     public static PIDConstants TRANSLATION_DRIVE_PID = new PIDConstants(1.0, 0.0, 0.0);
@@ -115,7 +126,7 @@ public static class Auto {
     public static double TAXI_AUTO_METERS_PER_SECOND = 1.0;
 
     /** The number of seconds that we want to taxi for */
-    public static double TAXI_AUTO_DURATION_SECONDS = 4.0;
+    public static double TAXI_AUTO_DURATION_SECONDS = 2.0;
 
     /** The amount of time we want/need to drive away from the amp in auto. */
     public static double DRIVE_AWAY_FROM_AMP_TIME = 2.0;
@@ -143,13 +154,15 @@ public static class Intake {
     public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 1;
 
     /** Speed we want to run the Intake at. */
-    public static final double INTAKE_MOTOR_SPEED = 1;
+    public static final double INTAKE_MOTOR_SPEED = .6;
 
     /** Inversion state for the intake follower motor. */
     public static final boolean INTAKE_FOLLOWER_INVERSE_STATE = false;
 
     /** Current limit (in amps) for the intake motor(s) */
     public static final int INTAKE_MOTOR_CURRENT_LIMIT = 40;
+
+    public static final IdleMode INTAKE_MOTOR_IDLE_MODE = IdleMode.kBrake;
   }
 
   /** Holds contstants for the Limelights. */

diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java
@@ -105,9 +105,18 @@ private static void configureDefaultKeybinds() {
     operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp);
     operatorController.rightTrigger().whileTrue(RobotContainer.fireNote);
 
-    pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented);
+    // pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented);
     pilotController.rightBumper().toggleOnTrue(RobotContainer.enterXMode);
-    pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint);
+    switch (Constants.Drive.currentDrivePerspective) {
+      case RobotOriented:
+        pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint);
+        break;
+      case FieldOriented:
+        pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint);
+        break;
+      default:
+        throw new IllegalStateException();
+    }
     /* TODO: angle / velocity steering toggle w/ right trigger (no issue) and boost on left bumper (issue 86) */
 
     keymapEntry.setString("Default");

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -24,6 +24,8 @@ public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
     robotContainer = new RobotContainer();
+
+    RobotContainer.startCamera.initialize();
   }
 
   @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,7 @@
 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;
@@ -39,6 +40,7 @@
 import frc.robot.commands.mailbox.FireNoteRoutine;
 import frc.robot.commands.mailbox.FireNoteRoutineNoLimitSwitch;
 import frc.robot.commands.mailbox.RunBelts;
+import frc.robot.subsystems.Camera;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.Intake;
@@ -91,6 +93,9 @@ public class RobotContainer {
   /** Singleton instance of {@link PDP} for the whole robot. */
   public static PDP pdp = new PDP();
 
+  /* TODO: JAVADOC */
+  public static Camera intakeCamera = new Camera(0);
+
   /*
    * ************
    * * COMMANDS *
@@ -100,10 +105,10 @@ public class RobotContainer {
   /** Singleton instance of robot oriented {@link DriveRobot} for the whole robot. */
   public static DriveRobot driveRobotOriented =
       new DriveRobot(
-          Controllers.getControllerAxisSupplier(
-              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.LeftY, true),
+          Controllers.getControllerAxisSupplier(
+              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.RightX, true),
           false,
@@ -113,10 +118,10 @@ public class RobotContainer {
   /** Singleton instance of field oriented {@link DriveRobot} for the whole robot. */
   public static DriveRobot driveFieldOriented =
       new DriveRobot(
-          Controllers.getControllerAxisSupplier(
-              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.LeftY, true),
+          Controllers.getControllerAxisSupplier(
+              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.RightX, true),
           true,
@@ -126,32 +131,32 @@ public class RobotContainer {
   /** Singleton instance of robot oriented sprint {@link DriveRobot} for the whole robot. */
   public static DriveRobot driveRobotOrientedSprint =
       new DriveRobot(
-          Controllers.getControllerAxisSupplier(
-              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.LeftY, true),
+          Controllers.getControllerAxisSupplier(
+              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.RightX, true),
           false);
 
   /** Singleton instance of field oriented sprint {@link DriveRobot} for the whole robot. */
   public static DriveRobot driveFieldOrientedSprint =
       new DriveRobot(
-          Controllers.getControllerAxisSupplier(
-              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.LeftY, true),
+          Controllers.getControllerAxisSupplier(
+              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.RightX, true),
           true);
 
   /** Singleton instance of {@link DriveFieldOrientedHeadingSnapping} for the whole robot. */
   public static DriveFieldOrientedHeadingSnapping driveFieldOrientedHeadingSnapping =
       new DriveFieldOrientedHeadingSnapping(
-          Controllers.getControllerAxisSupplier(
-              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.LeftY, true),
+          Controllers.getControllerAxisSupplier(
+              Controllers.pilotController, ControllerAxis.LeftX, true),
           Controllers.getControllerAxisSupplier(
               Controllers.pilotController, ControllerAxis.RightX, true),
           Controllers.headingSnappingUpSupplier,
@@ -230,6 +235,9 @@ 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 *
@@ -248,7 +256,16 @@ public RobotContainer() {
     Shuffleboard.getTab("Driver").add("Clear PDP sticky faults", clearPDPStickyFaults);
     Shuffleboard.getTab("Driver").add("Zero Gyro", zeroGyro);
 
-    drive.setDefaultCommand(driveRobotOriented);
+    switch (Constants.Drive.currentDrivePerspective) {
+      case RobotOriented:
+        drive.setDefaultCommand(driveRobotOriented);
+        break;
+      case FieldOriented:
+        drive.setDefaultCommand(driveFieldOriented);
+        break;
+      default:
+        throw new IllegalStateException();
+    }
   }
 
   private void configureAuto() {

diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoFieldOriented.java b/src/main/java/frc/robot/commands/auto/DriveAutoFieldOriented.java
@@ -38,13 +38,13 @@ public DriveAutoFieldOriented(Translation2d translation, double rotation) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-    drive.driveRobot(destination, rotation, true);
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    drive.driveRobot(destination, rotation, true);
+  }
 
   // Called once the command ends or is interrupted.
   @Override

diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoRobotOriented.java b/src/main/java/frc/robot/commands/auto/DriveAutoRobotOriented.java
@@ -38,13 +38,13 @@ public DriveAutoRobotOriented(Translation2d translation, double rotation) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-    drive.driveRobot(destination, rotation, false);
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    drive.driveRobot(destination, rotation, false);
+  }
 
   // Called once the command ends or is interrupted.
   @Override

diff --git a/src/main/java/frc/robot/commands/auto/TaxiAuto.java b/src/main/java/frc/robot/commands/auto/TaxiAuto.java
@@ -26,6 +26,6 @@ public TaxiAuto() {
     super(new WaitCommand(Constants.Auto.TAXI_AUTO_DURATION_SECONDS));
     addCommands(
         new DriveAutoRobotOriented(
-            new Translation2d(0, Constants.Auto.TAXI_AUTO_METERS_PER_SECOND), 0));
+            new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java
@@ -0,0 +1,42 @@
+// 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.subsystems;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Camera extends SubsystemBase {
+  private int port;
+  private UsbCamera camera;
+
+  /** Creates a new Camera. */
+  public Camera(int port) {
+    this.port = port;
+  }
+
+  public void startCamera() {
+    camera = CameraServer.startAutomaticCapture(port);
+    camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
+
+    System.out.println("Camera started");
+
+    Shuffleboard.getTab("Driver").add(camera).withSize(4, 4);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -36,6 +36,9 @@ public Intake() {
     intakeLower.setSmartCurrentLimit(Constants.Intake.INTAKE_MOTOR_CURRENT_LIMIT);
     intakeUpper.setSmartCurrentLimit(Constants.Intake.INTAKE_MOTOR_CURRENT_LIMIT);
 
+    intakeLower.setIdleMode(Constants.Intake.INTAKE_MOTOR_IDLE_MODE);
+    intakeUpper.setIdleMode(Constants.Intake.INTAKE_MOTOR_IDLE_MODE);
+
     intakeLower.setIdleMode(IdleMode.kBrake);
     intakeUpper.setIdleMode(IdleMode.kBrake);
 

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -19,7 +19,10 @@
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.DoubleTopic;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /** Subsystem for the Limelight 2+ that we use for vision. */
@@ -39,6 +42,8 @@ private String fmtPath(String end) {
     return "/" + name + "/" + end;
   }
 
+  private GenericEntry limelightHasTarget;
+
   /**
    * Creates a new Limelight.
    *
@@ -59,6 +64,14 @@ public Limelight(String name) {
         NetworkTableInstance.getDefault()
             .getDoubleArrayTopic(fmtPath("botpose"))
             .subscribe(defaultBotpos);
+
+    Shuffleboard.getTab("Driver")
+        .add(SendableCameraWrapper.wrap("Limelight", "http://10.9.37.5:5800/stream.mjpg"))
+        .withSize(4, 4);
+
+    /* TODO: CONSTANTS */
+    limelightHasTarget =
+        Shuffleboard.getTab("Driver").add("limelight has target", false).getEntry();
   }
 
   /* now its time for getter method chaingun, which I have to write manually because VS Code */
@@ -154,5 +167,6 @@ public void setLimelightPipeline(double pipeline) {
   public void periodic() {
     // This method will be called once per scheduler run
 
+    limelightHasTarget.setBoolean(hasValidTarget());
   }
 }