merge example fr this time (#23) & attempt co-authoring

* delete unused stuff

* Refactor SwerveDrive to use DriveConstants for kinematics and remove unused test files

* import stuff

Co-authored-by: Jack <[email protected]>

* simulation stuff

* format

* noice

* format

* added some of the more important docstrings.. the rest will be added later

* Examples yay (#21)

* maybe it will work

* Added more intake stuff and made it finally build!

* intake commands

* Cool elevator stuff

* more elevator stuff

* Hopefully i finish this soon

* idk how that got in there

* Fun arm things

* format

* Remove unnecessary blank line in MathUtil.java

* nerf intake to not use position for otb intake. delete arm example bc it is redundant with elevator existing

* format i think?

* update beta version

---------

Co-authored-by: Jack <[email protected]>
Co-authored-by: Max Luna <[email protected]>

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

@@ -6,11 +6,11 @@ public final class BuildConstants {
   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 String GIT_SHA = "9b2d52efc94f51550c86781dc4d51112afc44b13";
+  public static final String GIT_DATE = "2024-12-16 17:53:23 EST";
+  public static final String GIT_BRANCH = "ExamplesYay";
+  public static final String BUILD_DATE = "2024-12-18 17:41:48 EST";
+  public static final long BUILD_UNIX_TIME = 1734561708416L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/commands/intake/Outtake.java

@@ -0,0 +1,35 @@
+// This command ejects the note from the intake by reversing the rollors
+
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.Intake;
+
+public class Outtake extends Command {
+  private final Intake intakeSubsystem;
+
+  public Outtake(Intake intake) {
+    this.intakeSubsystem = intake;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    intakeSubsystem.setIntakeSpeed(-1);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/intake/RegularIntake.java

@@ -0,0 +1,35 @@
+// This command runs the rollers from the intake and grabs the note from the floor
+
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.Intake;
+
+public class RegularIntake extends Command {
+  private final Intake intakeSubsystem;
+
+  public RegularIntake(Intake intake) {
+    this.intakeSubsystem = intake;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    intakeSubsystem.setIntakeSpeed(1);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -43,18 +43,6 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) {
     return value;
   }
 
-  public static double modifyAxisCubed(DoubleSupplier supplierValue) {
-    double value = supplierValue.getAsDouble();
-
-    // Deadband
-    value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE);
-
-    // Cube the axis
-    value = Math.copySign(value * value * value, 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.
@@ -81,22 +69,4 @@ public static double[] modifyAxisPolar(
       Math.copySign(Math.pow(yInput, exponent), yInput)
     };
   }
-
-  public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) {
-    double xInput =
-        JoystickUtil.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);
-      double cubedHypotenuse = Math.pow(hypotenuse, 3);
-      xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput);
-      yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput);
-      return new double[] {xInput, yInput};
-    }
-    return new double[] {
-      Math.copySign(xInput * xInput * xInput, xInput),
-      Math.copySign(yInput * yInput * yInput, yInput)
-    };
-  }
 }

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -0,0 +1,39 @@
+/** This subsystem is an elevator that uses PID for its position. */
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Elevator extends SubsystemBase {
+  /** Creates a new Elevator. */
+  private ElevatorInterface elevatorInterface;
+
+  private ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged();
+
+  public Elevator(ElevatorInterface elevatorInterface) {
+    this.elevatorInterface = elevatorInterface;
+  }
+
+  public double getElevatorPosition() {
+    return elevatorInterface.getElevatorPosition();
+  }
+
+  public double getVolts() {
+    return elevatorInterface.getVolts();
+  }
+
+  public void setElevatorPosition(double position) {
+    elevatorInterface.setElevatorPosition(position);
+  }
+
+  public void setVolts(double volts) {
+    elevatorInterface.setVolts(volts);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    elevatorInterface.updateInputs(inputs);
+    Logger.processInputs("Elevator/", inputs);
+  }
+}

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.elevator;
+
+public class ElevatorConstants {
+  public static final int ELEVATOR_LEADER_MOTOR_ID = 0 - 9;
+  public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 0 - 9;
+
+  public static final double ELEVATOR_P = 0 - 9;
+  public static final double ELEVATOR_I = 0 - 9;
+  public static final double ELEVATOR_D = 0 - 9;
+
+  public static final double DRUM_RADIUS = 0 - 9;
+  public static final double ELEVATOR_GEAR_RATIO = 2;
+  public static final double ELEVATOR_CARRIAGE_MASS = 0 - 9;
+  public static final double ENCODER_CONVERSION_FACTOR = 71.81;
+  public static final double MIN_HEIGHT = 0;
+  public static final double MAX_HEIGHT = 0 - 9;
+
+  public static final double INTAKE_POSITION = MIN_HEIGHT;
+  public static final double SHOOT_AMP_POSITION = 0 - 9;
+  public static final double SHOOT_SPEAKER_POSITION = 0 - 9;
+  public static final double SHOOT_PASS_POSITION = 0 - 9;
+  public static final double ELEVATOR_OVER_DEFENSE = MAX_HEIGHT;
+
+  public static final double VELOCITY_METERS_PER_SECOND = 0 - 9;
+}

src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java

@@ -0,0 +1,37 @@
+package frc.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorInterface {
+
+  @AutoLog
+  public static class ElevatorInputs {
+    public double leaderMotorPosition = 0.0;
+
+    public double followerMotorPosition = 0.0;
+  }
+
+  /**
+   * Updates inputs for elevator for AdvantageKit to log
+   *
+   * @param inputs values related to the elevator
+   */
+  public default void updateInputs(ElevatorInputs inputs) {}
+
+  /**
+   * Gets the current position of the elevator
+   *
+   * @return
+   */
+  public default double getElevatorPosition() {
+    return 0.0;
+  }
+
+  public default void setElevatorPosition(double position) {}
+
+  public default void setVolts(double volts) {}
+
+  public default double getVolts() {
+    return 0.0;
+  }
+}

src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java

@@ -0,0 +1,61 @@
+package frc.robot.subsystems.elevator;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs;
+
+/** Add your docs here. */
+public class PhysicalElevator {
+  private TalonFX leaderMotor = new TalonFX(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID);
+  private TalonFX followerMotor = new TalonFX(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID);
+
+  StatusSignal<Angle> leaderPosition;
+  StatusSignal<Angle> followerPosition;
+
+  StatusSignal<Voltage> leaderAppliedVoltage;
+  StatusSignal<Voltage> followerAppliedVoltage;
+
+  public PhysicalElevator() {
+    leaderPosition = leaderMotor.getRotorPosition();
+    followerPosition = followerMotor.getRotorPosition();
+
+    leaderAppliedVoltage = leaderMotor.getMotorVoltage();
+    followerAppliedVoltage = followerMotor.getMotorVoltage();
+
+    TalonFXConfiguration elevatorConfig = new TalonFXConfiguration();
+
+    elevatorConfig.Slot0.kP = ElevatorConstants.ELEVATOR_P;
+    elevatorConfig.Slot0.kI = ElevatorConstants.ELEVATOR_I;
+    elevatorConfig.Slot0.kD = ElevatorConstants.ELEVATOR_D;
+
+    leaderMotor.getConfigurator().apply(elevatorConfig);
+    followerMotor.getConfigurator().apply(elevatorConfig);
+  }
+
+  public void updateInputs(ElevatorInputs inputs) {
+    inputs.leaderMotorPosition = leaderPosition.getValueAsDouble();
+    inputs.followerMotorPosition = followerPosition.getValueAsDouble();
+  }
+
+  public double getElevatorPosition() {
+    return leaderPosition.getValueAsDouble();
+  }
+
+  public void setElevatorPosition(double position) {
+    leaderMotor.setPosition(position);
+    followerMotor.setPosition(position);
+  }
+
+  public void setVolts(double volts) {
+    leaderMotor.setVoltage(volts);
+    followerMotor.setVoltage(volts);
+    ;
+  }
+
+  public double getVolts() {
+    return leaderMotor.getMotorVoltage().getValueAsDouble();
+  }
+}

src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+/** Add your docs here. */
+public class SimulatedElevator implements ElevatorInterface {
+  private ElevatorSim elevatorSim =
+      new ElevatorSim(
+          DCMotor.getFalcon500(2),
+          ElevatorConstants.ELEVATOR_GEAR_RATIO,
+          ElevatorConstants.ELEVATOR_CARRIAGE_MASS,
+          ElevatorConstants.DRUM_RADIUS,
+          ElevatorConstants.MIN_HEIGHT,
+          ElevatorConstants.MAX_HEIGHT,
+          true,
+          0.0);
+  private PIDController simPID;
+  private double currentVolts;
+
+  public SimulatedElevator() {
+    simPID =
+        new PIDController(
+            ElevatorConstants.ELEVATOR_P,
+            ElevatorConstants.ELEVATOR_I,
+            ElevatorConstants.ELEVATOR_D);
+  }
+
+  public void updateInputs(ElevatorInputs inputs) {
+    inputs.leaderMotorPosition = getElevatorPosition();
+    inputs.followerMotorPosition = getElevatorPosition();
+  }
+
+  public void setElevatorPosition(double position) {
+    setVolts(simPID.calculate(getElevatorPosition(), position));
+  }
+
+  public double getElevatorPosition() {
+    return elevatorSim.getPositionMeters();
+  }
+
+  public void setVolts(double volts) {
+    currentVolts = simPID.calculate(volts);
+    elevatorSim.setInputVoltage(currentVolts);
+  }
+
+  public double getVolts() {
+    return currentVolts;
+  }
+}

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

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+  private IntakeInterface intakeInterface;
+  private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged();
+
+  public Intake(IntakeInterface intakeInterface) {
+    this.intakeInterface = intakeInterface;
+  }
+
+  /**
+   * Sets the speed of the intake rollers
+   *
+   * @param speed intake roller speed (-1.0 to 1.0)
+   */
+  public void setIntakeSpeed(double speed) {
+    intakeInterface.setIntakeSpeed(speed);
+    Logger.recordOutput("OTBIntake/IntakeSpeed", speed);
+  }
+
+  @Override
+  public void periodic() {
+    intakeInterface.updateInputs(inputs);
+    Logger.processInputs("OTBIntake/", inputs);
+  }
+}