maybe it will work

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

@@ -1,17 +1,19 @@
 package frc.robot;
 
-/** Automatically generated file containing build version information. */
+/**
+ * Automatically generated file containing build version information.
+ */
 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 = 15;
-  public static final String GIT_SHA = "f6cf5383d95d1116029cbb69e2db020295f5c2bf";
-  public static final String GIT_DATE = "2024-11-09 19:29:39 EST";
-  public static final String GIT_BRANCH = "actual-unit-tests";
-  public static final String BUILD_DATE = "2024-11-09 19:50:39 EST";
-  public static final long BUILD_UNIX_TIME = 1731199839115L;
-  public static final int DIRTY = 1;
+  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 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;
 
-  private BuildConstants() {}
+  private BuildConstants(){}
 }

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

@@ -0,0 +1,72 @@
+package frc.robot.extras.util;
+
+import frc.robot.Constants.HardwareConstants;
+import java.util.function.DoubleSupplier;
+
+public class Util {
+
+  /**
+   * Deadbands a value to 0
+   *
+   * @param value Value to input
+   * @param deadband If the the input is within this value, the function will return 0.
+   * @return 0.0 if the value is within specified deadband range
+   */
+  private static double deadband(double value, double deadband) {
+    if (Math.abs(value) > deadband) {
+      if (value > 0.0) {
+        return (value - deadband) / (1.0 - deadband);
+      } else {
+        return (value + deadband) / (1.0 - deadband);
+      }
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Function to modify a singular joystick axis.
+   *
+   * @param supplierValue Supplies a double, usually by a joystick.
+   * @param exponent Exponent to raise the supplied value to.
+   * @return Returns the modified value.
+   */
+  public static double modifyAxis(DoubleSupplier supplierValue, int exponent) {
+    double value = supplierValue.getAsDouble();
+
+    // Deadband
+    value = deadband(value, HardwareConstants.DEADBAND_VALUE);
+
+    // Raise value to specified exponent
+    value = Math.copySign(Math.pow(value, exponent), value);
+
+    return value;
+  }
+
+  /**
+   * Converts the two axis coordinates to polar to get both the angle and radius, so they can work
+   * in a double[] list.
+   *
+   * @param xJoystick The supplied input of the xJoystick.
+   * @param yJoystick The supplied input of the yJoystick.
+   * @param exponent The exponent to raise the supplied value to.
+   * @return The modified axises of both joysticks in polar form.
+   */
+  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);
+    if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) {
+      double theta = Math.atan(xInput / yInput);
+      double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput);
+      double raisedHypotenuse = Math.pow(hypotenuse, exponent);
+      xInput = Math.copySign(Math.sin(theta) * raisedHypotenuse, xInput);
+      yInput = Math.copySign(Math.cos(theta) * raisedHypotenuse, yInput);
+      return new double[] {xInput, yInput};
+    }
+    return new double[] {
+      Math.copySign(Math.pow(xInput, exponent), xInput),
+      Math.copySign(Math.pow(yInput, exponent), yInput)
+    };
+  }
+}

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

@@ -0,0 +1,49 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+  private IntakeInterface io;
+  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);
+  }
+
+  /**
+   * Sets the speed of the intake rollers
+   *
+   * @param speed intake roller speed (-1.0 to 1.0)
+   */
+  public void setIntakeSpeed(double speed) {
+    io.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);
+    Logger.processInputs("OTBIntake", inputs);
+  }
+}

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

@@ -0,0 +1,39 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeInterface {
+  @AutoLog
+  public static class IntakeInputs {
+    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;
+  }
+
+  default void updateInputs(IntakeInputs inputs) {}
+
+  default void setPivotPosition(double position) {}
+
+  default void setIntakeSpeed(double speed) {}
+
+  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

@@ -0,0 +1,119 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.Constants.HardwareConstants;
+
+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();
+
+    intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    intakeConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
+
+    intakeConfig.CurrentLimits.StatorCurrentLimit = 0.0;
+    intakeConfig.CurrentLimits.StatorCurrentLimitEnable = false;
+    intakeConfig.CurrentLimits.SupplyCurrentLimit = 0.0;
+    intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = false;
+
+    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) {
+    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
+  public void setIntakeSpeed(double speed) {
+    intakeMotor.set(speed);
+  }
+
+  @Override
+  public double getIntakeSpeed() {
+    intakeMotor.getVelocity().refresh();
+    return intakeMotor.getVelocity().getValueAsDouble();
+  }
+}

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

@@ -0,0 +1,60 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+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.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
+  public void setIntakeSpeed(double speed) {
+    intakeSim.setInputVoltage(speed);
+  }
+
+  @Override
+  public double getIntakeSpeed() {
+    return intakeSim.getAngularVelocityRadPerSec() / (Math.PI * 2);
+  }
+
+  @Override
+  public double getPivotPosition() {
+    return pivotSim.getAngleRads() / (Math.PI * 2);
+  }
+}