Subsystem and IO class for double jointed arm

Author: k4limul

src/main/java/com/stuypulse/robot/subsystems/double_jointed_arm/DoubleJointedArm.java

@@ -0,0 +1,71 @@
+package com.stuypulse.robot.subsystems.double_jointed_arm;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Constants;
+import com.stuypulse.stuylib.math.SLMath;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DoubleJointedArm extends SubsystemBase {
+
+    private static DoubleJointedArm instance;
+
+    private DoubleJointedArmIO io;
+    private DoubleJointedArmIOInputsAutoLogged inputs = new DoubleJointedArmIOInputsAutoLogged();
+
+    public enum DoubleJointedArmState {
+        STOW(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10));
+
+        private Rotation2d shoulderTargetAngle;
+        private Rotation2d elbowTargetAngle;
+
+        private DoubleJointedArmState(Rotation2d shoulderTargetAngle, Rotation2d elbowTargetAngle){
+            this.shoulderTargetAngle = Rotation2d.fromDegrees(
+                SLMath.clamp(shoulderTargetAngle.getDegrees(), 
+                Constants.DoubleJointedArm.Shoulder.MIN_ANGLE.getDegrees(), 
+                Constants.DoubleJointedArm.Shoulder.MAX_ANGLE.getDegrees()));
+            this.elbowTargetAngle = Rotation2d.fromDegrees(
+                SLMath.clamp(elbowTargetAngle.getDegrees(), 
+                Constants.DoubleJointedArm.Elbow.MIN_ANGLE.getDegrees(), 
+                Constants.DoubleJointedArm.Elbow.MAX_ANGLE.getDegrees())); 
+        } 
+
+        public Rotation2d getShoulderTargetAngle(){
+            return this.shoulderTargetAngle;
+        }
+
+        public Rotation2d getElbowTargetAngle(){
+            return this.elbowTargetAngle;
+        }
+
+        private DoubleJointedArmState state;
+
+        public DoubleJointedArmState getState(){
+            return this.state;
+        }
+    }
+
+    @Override
+    public void periodic() {
+        // PUT ALL CALLS TO CONTROL METHODS HERE
+    }
+
+    /* LOGIC for controlling the double jointed arm: */
+
+    // 0. Measure system: angle, angular velocity, angular acceleration
+
+    // 1. Find M, C, G (mass inertia matrix, centrifugal + coriolis matrix, gravity torque matrix)
+
+    // 2. Given a current position (x, y) for the end effector on the double jointed arm, pathfind to a target position (x, y) and calculate the required angular position, velocity, and acceleration
+
+    // 3. Feed the current angular states and target angular states into the PID controllers for each joint
+
+    // 4. Calculate the three matrices using the angular states, which outputs a toruqe input
+
+    // 5. Using torque current control on the motor, feed the torque input into the motor
+}

src/main/java/com/stuypulse/robot/subsystems/double_jointed_arm/DoubleJointedArmIO.java

@@ -0,0 +1,57 @@
+package com.stuypulse.robot.subsystems.double_jointed_arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+
+public interface DoubleJointedArmIO {
+    @AutoLog
+    public class DoubleJointedArmIOInputs {
+        public DoubleJointedArmIOData data = 
+            new DoubleJointedArmIOData(false, false, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    }
+
+    record DoubleJointedArmIOData(
+        boolean shoulderMotorConnected,
+        boolean elbowMotorConnected,
+
+        double shoulderAngle,
+        double shoulderAngularVel,
+        double shoulderAngularAccel,
+        double shoulderAppliedVolts,
+        double shoulderCurrentAmps,
+
+        double elbowAngle,
+        double elbowAngularAccel,
+        double elbowAngularVel,
+        double elbowAppliedVolts,
+        double elbowCurrentAmps) {}
+
+    // STATES
+    public abstract void updateInputs(DoubleJointedArmIOInputs inputs);
+
+    // CONTROL
+    public abstract void runTorqueCurrentShoulder(double torqueCurrentAmps);
+    public abstract void runTorqueCurrentElbow(double torqueCurrentAmps);
+    public abstract void runVolts(double volts); // ONLY voltage override
+    public abstract void setTargetAngles(Rotation2d shoulderAngle, Rotation2d elbowAngle);
+    public abstract void setPID(double kP, double kI, double kD);
+
+    // BOOLEANS
+    public abstract boolean isShoulderAtTarget();
+    public abstract boolean isElbowAtTarget();
+    public abstract boolean isArmAtTarget();
+
+    // MATH
+    public abstract Matrix<N2, N2> calculateMMatrix(); // Mass Inertia Matrix
+    public abstract Matrix<N2, N2> calculateCMatrix(); // Centrifugal + Coriolis Matrix
+    public abstract Matrix<N2, N1> calculateGMatrix(); // Torque due to Gravity Matrix
+    public abstract Translation2d getEndPosition();
+
+    
+
+}