Fun arm things

src/main/java/frc/robot/subsystems/arm/Arm.java

@@ -0,0 +1,12 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public class Arm {
+  private ArmInterface armInterface;
+  private ArmInputsAutoLogged input = new ArmInputsAutoLogged();
+
+  public Arm(ArmInterface armInterface) {
+    this.armInterface = armInterface;
+  }
+}

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.arm;
+
+public class ArmConstants {
+  public static final int ARM_MOTOR_ID = 0 - 9;
+
+  public static final double ARM_P = 0 - 9;
+  public static final double ARM_I = 0 - 9;
+  public static final double ARM_D = 0 - 9;
+
+  public static final double ARM_GEAR_RATIO = 0 - 9;
+  public static final double ARM_INERTIA_MASS = 0 - 9;
+  public static final double ARM_LENGTH_METERS = 0 - 9;
+  public static final double MINIMUM_ANGLE_RADIANS = 0 - 9;
+  public static final double MAXIMUM_ANGLE_RADIANS = 0 - 9;
+}

src/main/java/frc/robot/subsystems/arm/ArmInterface.java

@@ -0,0 +1,20 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmInterface {
+
+  @AutoLog
+  public static class ArmInputs {
+    public double angle = 0.0;
+  }
+
+  public default void updateInputs(ArmInputs inputs) {}
+
+  public default void setAngle(double angle) {}
+
+  public default double getAngle() {
+    return 0.0;
+  }
+}
+

src/main/java/frc/robot/subsystems/arm/PhysicalArm.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+public class PhysicalArm implements ArmInterface {
+  private TalonFX armMotor = new TalonFX(ArmConstants.ARM_MOTOR_ID);
+
+  public PhysicalArm() {
+    TalonFXConfiguration armConfig = new TalonFXConfiguration();
+
+    armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    armConfig.Slot0.kP = ArmConstants.ARM_P;
+    armConfig.Slot0.kI = ArmConstants.ARM_I;
+    armConfig.Slot0.kD = ArmConstants.ARM_D;
+
+    armMotor.getConfigurator().apply(armConfig);
+  }
+
+  @Override
+  public void setAngle(double angle) {
+  }
+}

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

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+/** Add your docs here. */
+public class SimulatedElevator implements ArmInterface {
+  private SingleJointedArmSim armSim = new SingleJointedArmSim(DCMotor.getFalcon500(1), ArmConstants.ARM_GEAR_RATIO, ArmConstants.ARM_INERTIA_MASS, ArmConstants.ARM_LENGTH_METERS, ArmConstants.MINIMUM_ANGLE_RADIANS, ArmConstants.MAXIMUM_ANGLE_RADIANS, false, 0, null);
+  private PIDController pidSim;
+
+  public SimulatedElevator() {
+    pidSim = new PIDController(ArmConstants.ARM_P, ArmConstants.ARM_I, ArmConstants.ARM_D);
+  }
+}

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

@@ -1,3 +1,6 @@
+/**
+ * This subsystem is an elevator that uses PID for its position.
+ */
 package frc.robot.subsystems.elevator;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

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

@@ -1,6 +1,9 @@
 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;

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

@@ -11,8 +11,16 @@ public static class ElevatorInputs {
     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;
   }

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

@@ -12,8 +12,8 @@
 
 /** Add your docs here. */
 public class PhysicalElevator {
-  private TalonFX leaderMotor = new TalonFX(0);
-  private TalonFX followerMotor = new TalonFX(0);
+  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;

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

@@ -6,33 +6,33 @@
 
 /** Add your docs here. */
 public class SimulatedElevator implements ElevatorInterface {
-    private ElevatorSim simulatedElevator = 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) {
-        simulatedElevator.setState(simPID.calculate(position), ElevatorConstants.VELOCITY_METERS_PER_SECOND);
-    }
-
-    public double getElevatorPosition() {
-        return simulatedElevator.getPositionMeters();
-    }
-
-    public void setVolts(double volts) {
-        currentVolts = simPID.calculate(volts);
-        simulatedElevator.setInputVoltage(currentVolts);
-    }
-
-    public double getVolts() {
-        return currentVolts;
-    }
+  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) {
+    elevatorSim.setState(simPID.calculate(position), ElevatorConstants.VELOCITY_METERS_PER_SECOND);
+  }
+
+  public double getElevatorPosition() {
+    return elevatorSim.getPositionMeters();
+  }
+
+  public void setVolts(double volts) {
+    currentVolts = simPID.calculate(volts);
+    elevatorSim.setInputVoltage(currentVolts);
+  }
+
+  public double getVolts() {
+    return currentVolts;
+  }
 }