finished ArmIO Interface, worked on ArmIOSim

src/main/java/org/team1540/robot2025/Constants.java

@@ -1,5 +1,7 @@
 package org.team1540.robot2025;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation;
 
 /**
@@ -28,4 +30,35 @@ public enum Mode {
         /** Replaying from a log file. */
         REPLAY
     }
+
+    public static class Arm{
+        public static final double GEAR_RATIO = 28;
+        public static final double ARM_MOMENT_OF_INERTIA = 0; //TODO:
+        public static final double ARM_LENGTH_METERS = 0.411; //TODO: check please
+
+        // arm straight down is 0, min angle is 30
+        public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(30);
+        public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(330);
+        public static final Rotation2d STARTING_ANGLE = MIN_ANGLE;
+        public static final double MEASUREMENT_STANDARD_DEVIATIONS = 0; //TODO:
+
+        // PID loop constants
+        //TODO: what numbers are reasonable?
+        public static final double KP = 300;
+        public static final double KI = 50;
+        public static final double KD = 1;
+
+        //TODO: what values are reasonable?
+        public static final double MAX_VELOCITY = 30;
+        public static final double MAX_ACCELERATION = 3;
+
+        //TODO: get reasonable values
+        //TODO: try to understand
+        public static final double KS = 0.03;
+        public static final double KG = 0;
+        public static final double KV = 0.8;
+    }
+
+    public final static double LOOP_PERIODIC_SECS = 0.02; //TODO: is this value okay?
+
 }

src/main/java/org/team1540/robot2025/subsystems/arm/Arm.java

@@ -0,0 +1,10 @@
+package org.team1540.robot2025.subsystems.arm;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Arm extends SubsystemBase {
+
+    public void Arm(){
+
+    }
+}

src/main/java/org/team1540/robot2025/subsystems/arm/ArmIO.java

@@ -0,0 +1,20 @@
+package org.team1540.robot2025.subsystems.arm;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+public interface ArmIO {
+    @AutoLog
+    class ArmIOInputs{
+        //TODO: are these inputs okay? do I want any other ones?
+        public double velocityRPM = 0.0;
+        public double appliedVolts = 0.0;
+        public double currentAmps = 0.0;
+        public Rotation2d rotation2d = new Rotation2d();
+        public double tempCelsius = 0.0;
+    }
+
+    default void setVoltage(double voltage){}
+    default void updateInputs(ArmIOInputs inputs){}
+    default void setSetPoint(Rotation2d setPoint){};
+    default void configPID(double kP, double kI, double kD){};
+    default void setBrakeMode(boolean setBrake){};
+}

src/main/java/org/team1540/robot2025/subsystems/arm/ArmIOSim.java

@@ -0,0 +1,68 @@
+package org.team1540.robot2025.subsystems.arm;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import static org.team1540.robot2025.Constants.Arm.*;
+import static org.team1540.robot2025.Constants.LOOP_PERIODIC_SECS;
+
+public class ArmIOSim implements ArmIO{
+    //fields
+    private final SingleJointedArmSim armSim =
+            new SingleJointedArmSim(
+                    DCMotor.getKrakenX60(1),
+                    GEAR_RATIO,
+                    ARM_MOMENT_OF_INERTIA,
+                    ARM_LENGTH_METERS,
+                    MIN_ANGLE.getRadians(),
+                    MAX_ANGLE.getRadians(),
+                    true,
+                    STARTING_ANGLE.getRadians());
+
+    private double armAppliedVolts = 0.0;
+    private final ProfiledPIDController controller = new ProfiledPIDController(
+            KP, KI, KD, new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION));
+    private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV);
+    private boolean isClosedLoop;
+
+    //methods
+    public void setVoltage(double voltage){
+
+    }
+    public void updateInputs(ArmIOInputs inputs){
+        if(isClosedLoop){
+            armAppliedVolts = controller.calculate(armSim.getAngleRads(), inputs.rotation2d.getRadians())
+                    + feedforward.calculate(controller.getSetpoint().position, controller.getSetpoint().velocity);
+        }
+
+        armSim.setInputVoltage(armAppliedVolts);
+        armSim.update(LOOP_PERIODIC_SECS);
+
+        inputs.rotation2d = Rotation2d.fromRadians(armSim.getAngleRads());
+        inputs.appliedVolts = armAppliedVolts;
+        inputs.currentAmps = armSim.getCurrentDrawAmps();
+        inputs.velocityRPM = armSim.getVelocityRadPerSec()*60/(2*Math.PI); //converting to rpm;
+    }
+    public void setSetPoint(Rotation2d setPoint){
+
+    }
+    public void configPID(double kP, double kI, double kD){
+
+    }
+    public void setBrakeMode(boolean setBrake){
+
+    }
+
+//    @Override
+//    public void updateInputs(ArmIOInputs inputs) {
+//        //TODO:
+//    }
+//
+//    @Override
+//    public void setSetPointRads(double setPointRads){
+//        //TODO:
+//    }
+}