Merge remote-tracking branch 'origin/main' into intake-refactor

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

@@ -116,8 +116,18 @@ public static final class IntakeConstants {
   public static final class ShooterConstants {
     public static final int kTopShooterMotorPort = 35;
     public static final int kBottomShooterMotorPort = 20;
-  }
-
+  }    
+  public static class ClimberConstants {
+        public final static int leftForwardChannel = 0;
+        public final static int rightForwardChannel = 0;
+        public final static int leftReverseChannel = 1;
+        public final static int rightReverseChannel = 1;
+
+        public final static double minPressure = 50.0;
+        public final static double maxPressure = 120.0;
+
+
+    }
   public static final class VisionConstants {
     // TODO: Update cam pose relative to center of bot
     public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));

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

@@ -5,15 +5,12 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
@@ -25,12 +22,14 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
+import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.commands.DiskIntakeCommand;
 import frc.robot.commands.ExtendIntakeCommand;
 import frc.robot.commands.RetractIntakeCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.VisionSubsystem;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -42,7 +41,9 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-  public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
@@ -64,6 +65,8 @@ public RobotContainer() {
             new ReplanningConfig(true, true)),
         () -> false, m_robotDrive);
 
+    m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
+
     // Configure the trigger bindings
     configureBindings();
 
@@ -103,7 +106,7 @@ public RobotContainer() {
    */
   private void configureBindings() {
     new JoystickButton(m_driverController, Button.kStart.value)
-        .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+        .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive));
 
     // new JoystickButton(m_driverController, Button.kA.value).whileTrue(
     //     AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints(
@@ -116,6 +119,11 @@ private void configureBindings() {
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
 
+    new JoystickButton(m_operatorController, Button.kA.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive));
+    new JoystickButton(m_operatorController, Button.kB.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
+
     new Trigger(() -> {
       return m_driverController.getRightTriggerAxis() > 0.5;
     }).whileTrue(

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -0,0 +1,76 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+
+import frc.robot.Constants.ClimberConstants;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+
+public class ClimberSubsystem {
+
+    private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH,
+            ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel);
+    private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH,
+            ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel);
+
+    private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH);
+
+    private boolean enableCompressor = true;
+
+    public ClimberSubsystem() {
+        m_leftSolenoid.set(kOff);
+        m_rightSolenoid.set(kOff);
+        m_compressor.disable();
+        m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
+    }
+
+    // Runs once every tick (~20ms)
+    public void periodic() {
+    }
+
+    /**
+     * Sets the state of the solenoid to off
+     */
+    public void off() {
+        m_leftSolenoid.set(kOff);
+        m_rightSolenoid.set(kOff);
+    }
+
+    /**
+     * Extends both arms
+     */
+    public void forward() {
+        m_leftSolenoid.set(kForward);
+        m_rightSolenoid.set(kForward);
+    }
+
+    /**
+     * Retracts both arms
+     */
+    public void reverse() {
+        m_leftSolenoid.set(kReverse);
+        m_rightSolenoid.set(kReverse);
+    }
+    /*
+     * Toggles the state of the climber
+     */
+
+    public void toggle() {
+        m_leftSolenoid.toggle();
+        m_rightSolenoid.toggle();
+    }
+
+    // Toggles the state of the compressor (on/off)
+    public void toggleCompresor() {
+        enableCompressor = !enableCompressor;
+        if (enableCompressor) {
+            m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
+        } else {
+            m_compressor.disable();
+        }
+    }
+}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -4,8 +4,6 @@
 
 package frc.robot.subsystems;
 
-import java.util.Optional;
-
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.math.MathUtil;
@@ -71,8 +69,6 @@ public class DriveSubsystem extends SubsystemBase {
       m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
       VisionConstants.kVisionSTDDevs);
 
-  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
-
   private final Field2d m_field = new Field2d();
 
   /** Creates a new DriveSubsystem. */
@@ -96,14 +92,6 @@ public void periodic() {
     m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
         m_swerveModulePositions);
 
-    Optional<Measurement> latestReading = m_visionSubsystem.getMeasurement();
-
-    // SmartDashboard.putBoolean("reading present", latestReading.isPresent());
-
-    if (latestReading.isPresent()) {
-      m_poseEstimator.addVisionMeasurement(latestReading.get().pose.toPose2d(), latestReading.get().timestamp);
-    }
-
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
     SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
@@ -213,16 +201,20 @@ public void zeroHeading() {
     m_gyroAngle = 0;
   }
 
-  public void addVisionMeasurement(Pose2d pose, double timestamp) {
-    m_poseEstimator.addVisionMeasurement(pose, timestamp);
+  public void addVisionMeasurement(Measurement measurement) {
+    m_poseEstimator.addVisionMeasurement(measurement.pose.toPose2d(), measurement.timestamp);
+  }
+
+  public void autonDrive(ChassisSpeeds desiredChassisSpeeds) {
+    swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds);
   }
 
   /**
    * Sets the swerve ModuleStates. Overloaded for either auton builder or teleop.
    *
    * @param desiredStates The desired SwerveModule states.
    */
-  public void setModuleStates(SwerveModuleState[] desiredStates) {
+  private void setModuleStates(SwerveModuleState[] desiredStates) {
     SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
 
     m_frontLeft.setDesiredState(desiredStates[0]);
@@ -245,8 +237,4 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
         * Robot.kDefaultPeriod;
   }
 
-  public void autonDrive(ChassisSpeeds desiredChassisSpeeds) {
-    swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds);
-  }
-
 }

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -4,8 +4,10 @@
 
 package frc.robot.subsystems;
 
+import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.Optional;
+import java.util.List;
+import java.util.function.Consumer;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -49,12 +51,14 @@
  */
 public class VisionSubsystem extends SubsystemBase {
 
-  NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight");
+  private NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight");
 
   private final DoubleArraySubscriber m_botPose;
 
   private final IntegerSubscriber m_tv;
 
+  private List<Consumer<Measurement>> m_consumerList = new ArrayList<>(3);
+
   /** Creates a new Limelight. */
   public VisionSubsystem() {
     // Provide the limelight with the camera pose relative to the center of the
@@ -66,28 +70,41 @@ public VisionSubsystem() {
     m_tv = m_visionNetworkTable.getIntegerTopic("tv").subscribe(0);
   }
 
+  /** Add a consumer, which the vision subsystem will push an update to every time there is an updated measurement */
+  public void addConsumer(Consumer<Measurement> consumer) {
+    m_consumerList.add(consumer);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
 
+    Measurement latestMeasurement = getMeasurement();
+
+    if (latestMeasurement != null) {
+      for (Consumer<Measurement> consumer : m_consumerList) {
+        consumer.accept(latestMeasurement);
+      }
+    }
+
     SmartDashboard.putBoolean("Limelight Has Target", m_tv.get() == 1);
   }
 
-  public Optional<Measurement> getMeasurement() {
+  public Measurement getMeasurement() {
     TimestampedDoubleArray[] updates = m_botPose.readQueue();
 
     // If we have had no updates since the last time this method ran then return
     // nothing
     if (updates.length == 0) {
-      return Optional.empty();
+      return null;
     }
 
     TimestampedDoubleArray update = updates[updates.length - 1];
 
     // If the latest update is empty or we don't see an april tag then return
     // nothing
     if (Arrays.equals(update.value, new double[6]) || m_tv.get() == 0) {
-      return Optional.empty();
+      return null;
     }
 
     double x = update.value[0];
@@ -100,10 +117,10 @@ public Optional<Measurement> getMeasurement() {
     double timestamp = Timer.getFPGATimestamp() - (update.value[6] / 1000.0);
     Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw));
 
-    return Optional.of(new Measurement(
+    return new Measurement(
         timestamp,
         pose,
-        VisionConstants.kVisionSTDDevs));
+        VisionConstants.kVisionSTDDevs);
   }
 
   public static class Measurement {