feat: Intermidiet

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

@@ -100,10 +100,12 @@ public static final class IntakeConstants {
     public static final int kArmMotorID = 39;
     public static final int kArmEncoderChannel = 0;
 
+    // In degrees
     public static final double kIntakeLoweredAngle = 0;
-    public static final double kIntakeRaisedAngle = 120;
+    public static final double kIntakeRaisedAngle = 194;
 
-    public static final double kArmEncoderOffset = 0;
+    /** Encoder offset in rotations */
+    public static final double kArmEncoderOffset = 0.6692;
 
     public static final double kIntakeSpeed = 3.0;
     public static final double kTimeIntake = 5;
@@ -115,10 +117,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 final double kTimeShoot = 5;
-    public static final double kShooterMotorSpeed = 0;
-  }
-
+  }    
+  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

@@ -13,8 +13,6 @@
 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;
@@ -28,10 +26,12 @@
 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.IntakeCommand;
 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
@@ -41,17 +41,16 @@
  */
 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 SendableChooser<Command> autoChooser;
-
-  private boolean IntakeDropped = false;
-  private boolean lastAButton = false;
+
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
 
+  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
+  private final SendableChooser<Command> autoChooser;
+
   /**
    * The container for the robot. Contains subsystems, IO devices, and commands.
    */
@@ -66,6 +65,7 @@ public RobotContainer() {
         () -> m_intakeSubsystem.intakeTimedRun(Constants.IntakeConstants.kTimeIntake), m_intakeSubsystem));
 
     // All paths automatically
+
     autoChooser = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", autoChooser);
 
@@ -81,6 +81,8 @@ public RobotContainer() {
             new ReplanningConfig(true, true)),
         () -> false, m_robotDrive);
 
+    m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
+
     // Configure the trigger bindings
     configureBindings();
 
@@ -120,7 +122,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
@@ -135,6 +137,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(new IntakeCommand(m_intakeSubsystem));

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/IntakeSubsystem.java

@@ -10,6 +10,7 @@
 import com.revrobotics.Rev2mDistanceSensor;
 import com.revrobotics.Rev2mDistanceSensor.Port;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.Timer;
@@ -27,41 +28,43 @@ public class IntakeSubsystem extends SubsystemBase {
   private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
   private CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);
 
-  private PIDController m_armPID = new PIDController(0.5, 0, 0);
+  private PIDController m_armPID = new PIDController(0.0015, 0, 0);
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
   private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
+  private double m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
 
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
     // m_armMotor.setInverted(IntakeConstants.kArmMotorInverted);
     // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted);
 
     m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
-    m_armEncoder.setDistancePerRotation(2 * Math.PI);
+    m_armEncoder.setDistancePerRotation(360);
 
     m_intakeMotor.setIdleMode(IdleMode.kCoast);
-    m_armMotor.setIdleMode(IdleMode.kBrake);
+    m_armMotor.setIdleMode(IdleMode.kCoast);
 
+    // m_armPID.disableContinuousInput();
     m_armPID.setTolerance(0.05);
 
     // TODO: See if this is needed
     // m_distanceSensor.setAutomaticMode(true);
   }
 
   public void armExtend() {
-    m_armPID.setSetpoint(IntakeConstants.kIntakeLoweredAngle);
+    m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
 
     stopIntake();
 
     deployed = true;
   }
 
   public void armRetract() {
-    m_armPID.setSetpoint(IntakeConstants.kIntakeRaisedAngle);
+    m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
 
     stopIntake();
 
@@ -113,10 +116,10 @@ public void periodic() {
       armRetract();
     }
 
-    m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition()));
-    m_intakeMotor.set(m_intakeSpeed);
+    // m_armMotor.set(MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3));
+    // m_intakeMotor.set(m_intakeSpeed);
 
-    SmartDashboard.putNumber("Arm Angle", m_armEncoder.getAbsolutePosition());
+    SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
     SmartDashboard.putBoolean("Arm Deployed?", deployed);
     SmartDashboard.putBoolean("Have Note?", haveNote);
   }