Se/arm

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+
+public class ArmMoveToAngle extends InstantCommand{
+    private final Arm arm;
+    private final Rotation2d angle;
+
+    public ArmMoveToAngle(Rotation2d angle){
+        arm = Arm.getInstance();
+        this.angle = angle;
+    }
+
+    @Override
+    public void initialize(){
+        arm.setTargetAngle(angle);
+
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToFunnel extends ArmMoveToAngle{
+    public ArmMoveToFunnel(){
+        super(Settings.Arm.FUNNEL_ANGLE);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL2Back extends ArmMoveToAngle{
+    public ArmMoveToL2Back(){
+        super(Settings.Arm.L2_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL2Front extends ArmMoveToAngle{
+    public ArmMoveToL2Front(){
+        super(Settings.Arm.L2_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL3Back extends ArmMoveToAngle {
+    public ArmMoveToL3Back(){
+        super(Settings.Arm.L3_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL3Front extends ArmMoveToAngle{
+    public ArmMoveToL3Front(){
+        super(Settings.Arm.L3_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Back.java

@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmMoveToL4Back extends ArmMoveToAngle {
+    public ArmMoveToL4Back(){
+        super(Settings.Arm.L4_ANGLE_BACK);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL4Front.java

@@ -0,0 +1,14 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+
+
+public class ArmMoveToL4Front extends ArmMoveToAngle{
+    public ArmMoveToL4Front(){
+        super(Settings.Arm.L4_ANGLE_FRONT);
+    }
+    @Override
+    public void initialize(){
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -13,6 +13,12 @@ public interface Gamepad {
         int DEBUGGER = 2;
     }
 
+
+    public interface Arm {
+        int ARM_MOTOR = 0;
+        int ARM_ENCODER = 0;
+    }
+
     // Set values later
     public interface Shooter {
         int MOTOR = 0;

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -7,13 +7,14 @@
 
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-
 public interface Settings { 
     double DT = 0.020; // 20ms Differential Time
 
@@ -92,4 +93,45 @@ public interface Simulation {
             double SCALE_FACTOR = 0.5 + 2.5/77;
         }
     }
-}
+
+      public interface Arm {
+
+        public interface PID {
+            SmartNumber kP = new SmartNumber("Arm/PID/kP", 0.0);
+            SmartNumber kI = new SmartNumber("Arm/PID/kI", 0);
+            SmartNumber kD = new SmartNumber("Arm/PID/kD", 0);
+        }
+
+        public interface FF {
+            SmartNumber kS = new SmartNumber("Arm/PID/kP", 0.0);
+            SmartNumber kV = new SmartNumber("Arm/PID/kI", 0);
+            SmartNumber kA = new SmartNumber("Arm/PID/kD", 0);
+            SmartNumber kG = new SmartNumber("Arm/PID/kG", 0);
+        }
+
+        Rotation2d L2_ANGLE_FRONT = Rotation2d.fromDegrees(0);
+        Rotation2d L3_ANGLE_FRONT = Rotation2d.fromDegrees(0);
+        Rotation2d L4_ANGLE_FRONT = Rotation2d.fromDegrees(0);
+
+        Rotation2d L2_ANGLE_BACK = Rotation2d.fromDegrees(0);
+        Rotation2d L3_ANGLE_BACK = Rotation2d.fromDegrees(0);
+        Rotation2d L4_ANGLE_BACK = Rotation2d.fromDegrees(0);
+
+        Rotation2d FUNNEL_ANGLE = Rotation2d.fromDegrees(0);
+        Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(0);
+        double GEAR_RATIO = 0;
+        double ARM_OFFSET = 0;
+        double ENCODER_OFFSET = 0;
+        SmartNumber PID_RAMPING = new SmartNumber("Arm/PID_RAMP",0);
+        SmartNumber FF_RAMPING = new SmartNumber("Arm/FF_RAMP", 0);
+        SmartNumber CURRENT_RAMPING = new SmartNumber("Arm/CURRENT_RAMP",0);
+
+
+        public interface MotionMagic{
+            double MAX_VEL = 0;
+            double MAX_ACCEL = 0;
+            double JERK = 0;
+        }
+      }
+
+  }

src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java

@@ -0,0 +1,25 @@
+package com.stuypulse.robot.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public abstract class Arm extends SubsystemBase {
+
+    public static final Arm instance;
+
+    static {
+        instance = new ArmImpl();
+    }
+
+    public static Arm getInstance() {
+        return instance;
+    }
+
+    public abstract void setTargetAngle(Rotation2d TargetAngle);
+
+    public abstract Rotation2d getTargetAngle();
+
+    public abstract Rotation2d getArmAngle();
+
+}

src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java

@@ -0,0 +1,101 @@
+package com.stuypulse.robot.subsystems.arm;
+
+
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Settings;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ArmImpl extends Arm {
+
+    private Rotation2d targetAngle;
+
+    private TalonFX armMotor;
+
+    private CANcoder armEncoder;
+
+
+    public ArmImpl() {
+
+        targetAngle = Rotation2d.fromDegrees(0.0);
+        armMotor = new TalonFX(Ports.Arm.ARM_MOTOR);
+        armEncoder = new CANcoder(Ports.Arm.ARM_ENCODER);
+
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        Slot0Configs slot0 = new Slot0Configs();
+
+        slot0.kP = Settings.Arm.PID.kP.getAsDouble();
+        slot0.kI = Settings.Arm.PID.kI.getAsDouble();
+        slot0.kD = Settings.Arm.PID.kD.getAsDouble();
+
+        slot0.kS = Settings.Arm.FF.kS.getAsDouble();
+        slot0.kV = Settings.Arm.FF.kV.getAsDouble();
+        slot0.kA = Settings.Arm.FF.kA.getAsDouble();
+        slot0.kG = Settings.Arm.FF.kG.getAsDouble();
+        slot0.GravityType = GravityTypeValue.Arm_Cosine;
+
+        config.Slot0 = slot0;
+        config.Feedback.SensorToMechanismRatio = Settings.Arm.GEAR_RATIO;
+        // config.Feedback.RotorToSensorRatio = 0.0;
+        config.Feedback.FeedbackRemoteSensorID = armEncoder.getDeviceID();
+        config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+
+        MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
+
+        motionMagicConfigs.MotionMagicCruiseVelocity = Settings.Arm.MotionMagic.MAX_VEL; // Target cruise velocity of 80 rps
+        motionMagicConfigs.MotionMagicAcceleration = Settings.Arm.MotionMagic.MAX_ACCEL; // Target acceleration of 160 rps/s (0.5 seconds)
+        motionMagicConfigs.MotionMagicJerk = Settings.Arm.MotionMagic.JERK; 
+
+        MagnetSensorConfigs magnet_config = new MagnetSensorConfigs();
+        magnet_config.MagnetOffset = Settings.Arm.ENCODER_OFFSET;
+
+        config.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Arm.PID_RAMPING.getAsDouble(); 
+        config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Arm.FF_RAMPING.getAsDouble(); 
+        config.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Arm.PID_RAMPING.getAsDouble();
+        config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Arm.FF_RAMPING.getAsDouble();
+
+
+        armMotor.getConfigurator().apply(config);
+        armMotor.getConfigurator().apply(motionMagicConfigs);
+        armEncoder.getConfigurator().apply(magnet_config);
+
+    }
+
+    public void setTargetAngle(Rotation2d targetAngle) {
+        this.targetAngle = targetAngle;
+    }
+
+    public Rotation2d getTargetAngle() {
+        return targetAngle;
+    }
+
+
+    public Rotation2d getArmAngle() {
+        return Rotation2d.fromRotations(armMotor.getPosition().getValueAsDouble());
+    }
+
+    @Override
+    public void periodic() { 
+
+        MotionMagicVoltage armOutput = new MotionMagicVoltage(getTargetAngle().getRotations());
+        // armMotor.setControl(new PositionVoltage(getTargetAngle().getRotations()));
+        armMotor.setControl(armOutput);
+
+        SmartDashboard.putNumber("Arm/targetAngle", getTargetAngle().getDegrees());
+        SmartDashboard.putNumber("Arm/currentAngle",getArmAngle().getDegrees());
+
+    }
+}
+

vendordeps/PathplannerLib.json → vendordeps/PathplannerLib-2025.2.2.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib.json",
+    "fileName": "PathplannerLib-2025.2.2.json",
     "name": "PathplannerLib",
-    "version": "2025.1.1",
+    "version": "2025.2.2",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.1.1"
+            "version": "2025.2.2"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.1.1",
+            "version": "2025.2.2",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,