Feature/arm 2024

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

@@ -18,6 +18,7 @@
 
 public class Arm extends SubsystemBase {
     private static Arm INSTANCE;
+    private final ArmInputsLogged inputs = new ArmInputsLogged();
     private final ArmIO io;
     private final Mechanism2d mechanism = new Mechanism2d(
             3, 3
@@ -27,24 +28,24 @@ public class Arm extends SubsystemBase {
             new MechanismLigament2d("Shoulder", ArmConstants.SHOULDER_LENGTH, 0)
     );
     private final MechanismLigament2d elbow = shoulder.append(
-            new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0, 10, new Color8Bit(Color.kPurple))
+            new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0)
     );
     private Command lastCommand = null;
     private Command currentCommand = null;
     private boolean changedToDefaultCommand = false;
 
-    private Arm(ArmIO io) {
-        this.io = io;
+
+    private Arm() {
+        if (Robot.isReal()) {
+            io = new ArmIOReal();
+        } else {
+            io = new ArmIOReal();
+        }
     }
 
     public static Arm getINSTANCE() {
         if (INSTANCE == null) {
-            if (Robot.isReal()) {
-                INSTANCE = new Arm(new ArmIOSim(inputs));
-
-            } else {
-                INSTANCE = new Arm(new ArmIOSim(inputs));
-            }
+            INSTANCE = new Arm();
         }
         return INSTANCE;
     }
@@ -93,7 +94,7 @@ public Translation2d getEndPosition() {
         return ArmIO.armKinematics.forwardKinematics(shoulderAngle, shoulderAngle + elbowAngle - Math.PI);
     }
 
-    public ArmIO.ArmInputs getInputs() {
+    public ArmInputsLogged getInputs() {
         return inputs;
     }
 

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.arm;
 
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.TalonFXInvertType;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -45,6 +46,8 @@ public class ArmConstants {
     public static final double END_POSITION_LOWER_X_LIMIT = 0; //[cm]
     public static final double SHOULDER_ABSOLUTE_ENCODER_OFFSET = 0.4090 - SHOULDER_ZERO_POSITION / 360.0;
     public static final double ELBOW_ABSOLUTE_ENCODER_OFFSET = 0.57396956434 - ELBOW_ZERO_POSITION / 360.0;
+    public static final SupplyCurrentLimitConfiguration CURRENT_LIMIT = new SupplyCurrentLimitConfiguration(true, 50, 0, 0);
+
 
     //PID
     public static final double shoulderP = 4.0;

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

@@ -5,6 +5,7 @@
 
 public interface ArmIO {
 
+
     ArmKinematics armKinematics = new ArmKinematics(ArmConstants.SHOULDER_LENGTH, ArmConstants.ELBOW_LENGTH);
 
     default void setShoulderPower(double power) {
@@ -29,27 +30,12 @@ default void setElbowP(double kP) {
     default void updateInputs() {
     }
 
+    String getSubsystemName();
+
+    void updateInputs(ArmInputsLogged inputs);
+
     enum ControlMode {
         PRECENT_OUTPUT,
         POSITION
     }
-
-    @AutoLog
-    class ArmInputs {
-
-        public double shoulderAngleSetPoint = 0;
-        double shoulderAppliedVoltage = 0;
-        double elbowAppliedVoltage = 0;
-        double shoulderAppliedCurrent = 0;
-        double elbowAppliedCurrent = 0;
-        double[] shoulderTipPose = new double[2];
-        double[] endEffectorPositionSetPoint = new double[2];
-        double shoulderAngle = 0;
-        double elbowAngleSetpoint = 0;
-        double elbowAngleRelative = 0;
-        double elbowAngleAbsolute = 0;
-        Translation2d endEffectorPose;
-        ControlMode shoulderControlMode = ControlMode.PRECENT_OUTPUT;
-        ControlMode elbowControlMode = ControlMode.PRECENT_OUTPUT;
-    }
 }