Added more intake stuff and made it finally build!

src/main/java/frc/robot/BuildConstants.java

@@ -7,13 +7,13 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "4829-BaseRobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 9;
-  public static final String GIT_SHA = "489425d396236751b7dfe7735b944f8b36beac20";
-  public static final String GIT_DATE = "2024-11-10 12:24:24 EST";
+  public static final int GIT_REVISION = 10;
+  public static final String GIT_SHA = "a81461e343ffe77aad20f5e40904bf123e067fae";
+  public static final String GIT_DATE = "2024-11-15 17:55:39 EST";
   public static final String GIT_BRANCH = "ExamplesYay";
-  public static final String BUILD_DATE = "2024-11-15 17:54:44 EST";
-  public static final long BUILD_UNIX_TIME = 1731711284284L;
-  public static final int DIRTY = 0;
+  public static final String BUILD_DATE = "2024-11-18 17:29:09 EST";
+  public static final long BUILD_UNIX_TIME = 1731968949047L;
+  public static final int DIRTY = 1;
 
   private BuildConstants(){}
 }

src/main/java/frc/robot/extras/util/JoystickUtil.java

@@ -1,9 +1,9 @@
 package frc.robot.extras.util;
 
-import frc.robot.Constants.HardwareConstants;
+import frc.robot.Constants.JoystickConstants;
 import java.util.function.DoubleSupplier;
 
-public class Util {
+public class JoystickUtil {
 
   /**
    * Deadbands a value to 0
@@ -35,7 +35,7 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) {
     double value = supplierValue.getAsDouble();
 
     // Deadband
-    value = deadband(value, HardwareConstants.DEADBAND_VALUE);
+    value = deadband(value, JoystickConstants.DEADBAND_VALUE);
 
     // Raise value to specified exponent
     value = Math.copySign(Math.pow(value, exponent), value);
@@ -54,8 +54,8 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) {
    */
   public static double[] modifyAxisPolar(
       DoubleSupplier xJoystick, DoubleSupplier yJoystick, int exponent) {
-    double xInput = deadband(xJoystick.getAsDouble(), HardwareConstants.DEADBAND_VALUE);
-    double yInput = deadband(yJoystick.getAsDouble(), HardwareConstants.DEADBAND_VALUE);
+    double xInput = deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
+    double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
     if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) {
       double theta = Math.atan(xInput / yInput);
       double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput);

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -5,22 +5,11 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
-  private IntakeInterface io;
+  private IntakeInterface intakeInterface;
   private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged();
 
-  public Intake(IntakeInterface io) {
-    this.io = io;
-  }
-
-  /**
-   * sets the angle of the pivot in degrees
-   *
-   * @param angle desired angle in degrees
-   */
-  public void setPivotAngle(double angle) {
-    double angleRots = Units.degreesToRotations(angle);
-    io.setPivotPosition(angleRots);
-    Logger.recordOutput("OTBIntake/Pivot", angleRots);
+  public Intake(IntakeInterface intakeInterface) {
+    this.intakeInterface = intakeInterface;
   }
 
   /**
@@ -29,21 +18,13 @@ public void setPivotAngle(double angle) {
    * @param speed intake roller speed (-1.0 to 1.0)
    */
   public void setIntakeSpeed(double speed) {
-    io.setIntakeSpeed(speed);
+    intakeInterface.setIntakeSpeed(speed);
     Logger.recordOutput("OTBIntake/Intake", speed);
   }
 
-  public void setPivotSpeed(double speed) {
-    io.setPivotSpeed(speed);
-  }
-
-  public void getPivotPosition() {
-    io.getPivotPosition();
-  }
-
   @Override
   public void periodic() {
-    io.updateInputs(inputs);
+    intakeInterface.updateInputs(inputs);
     Logger.processInputs("OTBIntake", inputs);
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.intake;
+
+// This cool file contains constants for our intake
+public final class IntakeConstants {
+  public static final int INTAKE_MOTOR_ID = 0;
+  public static final int LEFT_INTAKE_PIVOT_MOTOR_ID = 62;
+  public static final int RIGHT_INTAKE_PIVOT_MOTOR_ID = 0 - 9;
+
+  public static final int NOTE_SENSOR_ID = 0 - 9;
+
+  public static final double INTAKE_PIVOT_GEAR_RATIO = 8.0;
+
+  public static final double INTAKE_PIVOT_OUT = 0 - 9;
+  public static final double INTAKE_PIVOT_IN = 0 - 9;
+
+  public static final double INTAKE_PIVOT_NEUTRAL_SPEED = 0.0;
+
+  public static final double OUTTAKE_SPEED = 0 - 9;
+  public static final double INTAKE_SPEED = 0.8;
+  public static final double INTAKE_NEUTRAL_SPEED = 0.0;
+  public static final double FLAPPER_SPEED = 1.0;
+
+  public static final double INTAKE_STATOR_LIMIT = 60;
+  public static final double INTAKE_SUPPLY_LIMIT = 40;
+  public static final boolean INTAKE_STATOR_ENABLE = true;
+  public static final boolean INTAKE_SUPPLY_ENABLE = true;
+}

src/main/java/frc/robot/subsystems/intake/IntakeInterface.java

@@ -5,35 +5,32 @@
 public interface IntakeInterface {
   @AutoLog
   public static class IntakeInputs {
-    public boolean isConnected = true;
+    public boolean isConnected = true; //
 
     // intake motor
     public double intakeVelocity = 0.0;
     public double intakeTemp = 0.0;
     public double intakeAppliedVolts = 0.0;
     public double intakeCurrentAmps = 0.0;
-
-    // pivot
-    public double pivotPosition = 0.0;
-    public double pivotVelocity = 0.0;
-    public double pivotTemp = 0.0;
-    public double pivotAppliedVolts = 0.0;
-    public double pivotCurrentAmps = 0.0;
   }
 
+  /**
+   * Updates intake values
+   * @param inputs
+   */
   default void updateInputs(IntakeInputs inputs) {}
 
-  default void setPivotPosition(double position) {}
-
+  /**
+   * Sets the intake speed
+   * @param speed The intake speed value from -1 to 1 (negative being reversed)
+   */
   default void setIntakeSpeed(double speed) {}
 
+  /**
+   * Gets the current speed of the intake
+   * @return Current velocity of the intake
+   */
   default double getIntakeSpeed() {
     return 0.0;
   }
-
-  default void setPivotSpeed(double speed) {}
-
-  default double getPivotPosition() {
-    return 0.0;
-  }
 }

src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java

@@ -14,15 +14,10 @@
 
 public class PhysicalIntake implements IntakeInterface {
   private final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
-  private final TalonFX leaderPivotMotor = new TalonFX(IntakeConstants.LEFT_INTAKE_PIVOT_MOTOR_ID);
-  private final TalonFX followerPivotMotor =
-      new TalonFX(IntakeConstants.RIGHT_INTAKE_PIVOT_MOTOR_ID);
 
   private final MotionMagicVoltage mmPositionRequest = new MotionMagicVoltage(0);
 
   private final StatusSignal<AngularVelocity> intakeVelocity;
-  private final StatusSignal<Angle> pivotPosition;
-  private final StatusSignal<AngularVelocity> pivotVelocity;
 
   public PhysicalIntake() {
     TalonFXConfiguration intakeConfig = new TalonFXConfiguration();
@@ -38,72 +33,12 @@ public PhysicalIntake() {
 
     intakeMotor.getConfigurator().apply(intakeConfig);
 
-    TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
-
-    pivotConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-    pivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    pivotConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
-
-    pivotConfig.CurrentLimits.StatorCurrentLimit = 0.0;
-    pivotConfig.CurrentLimits.StatorCurrentLimitEnable = false;
-    pivotConfig.CurrentLimits.SupplyCurrentLimit = 0.0;
-    pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = false;
-
-    pivotConfig.Slot0.kP = 0.0;
-    pivotConfig.Slot0.kI = 0.0;
-    pivotConfig.Slot0.kD = 0.0;
-    pivotConfig.Slot0.kS = 0.0;
-    pivotConfig.Slot0.kV = 0.0;
-    pivotConfig.Slot0.kA = 0.0;
-    pivotConfig.Slot0.kG = 0.0;
-    pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
-
-    pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 0.0;
-    pivotConfig.MotionMagic.MotionMagicAcceleration = 0.0;
-
-    // pivotConfig.Feedback.FeedbackRotorOffset = 0.0;
-    // pivotConfig.Feedback.RotorToSensorRatio = 0.0;
-    pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
-    pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.0;
-    pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
-    pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0.0;
-    pivotConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
-
-    leaderPivotMotor.setPosition(0);
-    followerPivotMotor.setPosition(0);
-
-    leaderPivotMotor.getConfigurator().apply(pivotConfig);
-    pivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    followerPivotMotor.getConfigurator().apply(pivotConfig);
-
     intakeVelocity = intakeMotor.getVelocity();
-    pivotPosition = leaderPivotMotor.getRotorPosition();
-    pivotVelocity = leaderPivotMotor.getVelocity();
   }
 
   @Override
-  public void updateInputs(IntakeInterfaceInputs inputs) {
+  public void updateInputs(IntakeInputs inputs) {
     inputs.intakeVelocity = intakeVelocity.getValueAsDouble();
-    inputs.pivotPosition = pivotPosition.getValueAsDouble();
-    inputs.pivotVelocity = pivotVelocity.getValueAsDouble();
-  }
-
-  @Override
-  public void setPivotPosition(double position) {
-    leaderPivotMotor.setControl(mmPositionRequest.withPosition(position));
-    followerPivotMotor.setControl(mmPositionRequest.withPosition(position));
-  }
-
-  @Override
-  public void setPivotSpeed(double speed) {
-    leaderPivotMotor.set(speed);
-    followerPivotMotor.set(speed);
-  }
-
-  @Override
-  public double getPivotPosition() {
-    leaderPivotMotor.getRotorPosition().refresh();
-    return leaderPivotMotor.getRotorPosition().getValueAsDouble();
   }
 
   @Override

src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java

@@ -6,41 +6,19 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import static edu.wpi.first.units.Units.*;
+
 
 public class SimulatedIntake implements IntakeInterface {
   DCMotorSim intakeSim = new DCMotorSim(null, DCMotor.getFalcon500(1), 0);
-  SingleJointedArmSim pivotSim =
-      new SingleJointedArmSim(
-          DCMotor.getFalcon500(2), IntakeConstants.INTAKE_PIVOT_GEAR_RATIO, 0, 0, 0, 0, false, 0);
-  private final Constraints pivotConstraints = new Constraints(0, 0);
-  private final ProfiledPIDController pivotController =
-      new ProfiledPIDController(0, 0, 0, pivotConstraints);
-
-  private final ArmFeedforward pivotFF = new ArmFeedforward(0, 0, 0, 0);
-
   private double intakeAppliedVolts = 0.0;
-  private double pivotAppliedVolts = 0.0;
 
   public SimulatedIntake(IntakeInputs inputs) {
     intakeSim.update(0.02);
-    pivotSim.update(0.02);
-
-    inputs.intakeVelocity = intakeSim.getAngularVelocityRadPerSec() / (Math.PI * 2);
+
+    inputs.intakeVelocity = RadiansPerSecond.of(intakeSim.getAngularVelocityRadPerSec()).in(RotationsPerSecond);
     inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
     inputs.intakeAppliedVolts = intakeAppliedVolts;
-
-    inputs.pivotPosition = pivotSim.getAngleRads() / (Math.PI * 2);
-    inputs.pivotVelocity = pivotSim.getVelocityRadPerSec() / (Math.PI * 2);
-    inputs.pivotAppliedVolts = pivotAppliedVolts;
-    inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps();
-  }
-
-  @Override
-  public void setPivotPosition(double position) {
-    pivotSim.setInputVoltage(
-        pivotController.calculate(position, pivotSim.getAngleRads() / (Math.PI * 2))
-            + pivotFF.calculate(
-                pivotController.getSetpoint().position, pivotController.getSetpoint().velocity));
   }
 
   @Override
@@ -50,11 +28,6 @@ public void setIntakeSpeed(double speed) {
 
   @Override
   public double getIntakeSpeed() {
-    return intakeSim.getAngularVelocityRadPerSec() / (Math.PI * 2);
-  }
-
-  @Override
-  public double getPivotPosition() {
-    return pivotSim.getAngleRads() / (Math.PI * 2);
+    return RadiansPerSecond.of(intakeSim.getAngularVelocityRadPerSec()).in(RotationsPerSecond);
   }
 }

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -18,8 +18,8 @@
 import frc.robot.extras.util.DeviceCANBus;
 import frc.robot.extras.util.TimeUtil;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
-import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged;
 import frc.robot.subsystems.swerve.gyroIO.GyroInterface;
+import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged;
 import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged;

src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java

@@ -54,16 +54,17 @@ static OdometryThread createInstance(DeviceCANBus canBus) {
               registeredInputs.toArray(new OdometryDoubleInput[0]),
               registeredStatusSignals.toArray(new BaseStatusSignal[0]));
       case SIM -> new OdometryThreadSim();
-      case REPLAY -> inputs -> {};
+      //case REPLAY -> inputs -> {};
+      default -> throw new IllegalArgumentException("Unexpected value: " + Robot.CURRENT_ROBOT_MODE);
     };
   }
 
   @AutoLog
-  class OdometryThreadInputs {
+  public class OdometryThreadInputs {
     public double[] measurementTimeStamps = new double[0];
   }
 
-  void updateInputs(OdometryThreadInputs inputs);
+  default void updateInputs(OdometryThreadInputs inputs) {}
 
   default void start() {}
 

src/main/java/frc/robot/subsystems/vision/VisionIOReal.java → src/main/java/frc/robot/subsystems/vision/PhysicalVision.java

@@ -14,7 +14,7 @@
 import java.util.concurrent.TimeUnit;
 import java.util.concurrent.atomic.AtomicBoolean;
 
-public class VisionIOReal implements VisionIO {
+public class PhysicalVision implements VisionInterface {
 
   private Pose2d lastSeenPose = new Pose2d();
   private double headingDegrees = 0;
@@ -28,7 +28,7 @@ public class VisionIOReal implements VisionIO {
    */
   private PoseEstimate[] limelightEstimates;
 
-  public VisionIOReal() {
+  public PhysicalVision() {
     limelightEstimates = new PoseEstimate[3];
     for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) {
       limelightThreads.put(limelightNumber, new AtomicBoolean(true));
@@ -37,7 +37,7 @@ public VisionIOReal() {
   }
 
   @Override
-  public void updateInputs(VisionIOInputs inputs) {
+  public void updateInputs(VisionInputs inputs) {
     inputs.camerasAmount = limelightEstimates.length;
     inputs.cameraConnected = true;