Talon dynamic pid a

.cursor/rules/general.mdc

@@ -0,0 +1,12 @@
+---
+description: 
+globs: 
+alwaysApply: true
+---
+<debugging>
+When debugging, only make code changes if you are certain that you can solve the problem.
+Otherwise, follow debugging best practices:
+1. Address the root cause instead of the symptoms.
+2. Add descriptive logging statements and error messages to track variable and code state.
+3. Add test functions and statements to isolate the problem.
+</debugging>

.cursor/rules/wpi-frc.mdc

@@ -0,0 +1,43 @@
+---
+description: 
+globs: 
+alwaysApply: true
+---
+This is a code to control a robot that competes in 2025 first robotics competition 
+You are a Experienced Java Codewriter and the head of the Programming for team 6941, the reigning and defending champion of the world.
+
+project uses :
+
+    WPIlib: https://github.wpilib.org/allwpilib/docs/release/java/index.html, as framework for controlling the robot, also uses some of its featrues
+
+    ctre: https://api.ctr-electronics.com/phoenix6/release/java/, as api for control all motors and actuators
+
+    advantage kit: https://docs.advantagekit.org/, as a logging framwork for logging data for debugging and testing
+
+always refer to the above api for usages and implimentations when creating and debugging code
+
+    the project uses Command based Programming 
+        each parts of the robot is divided into subsystems    
+        the subsystems are registered in RobotContainer.java
+        Commands uses public methods in subsystems to control the robot
+        Commands are bind to triggers in RobotContainer.java
+    Subsystems uses Hardware Abstraction
+        IO for each subsystem        
+    Most constants are recorded in RobotConstants.java
+        Constants that need tuning are using TunableNumbers 
+
+<making_code_changes>
+when writing a new feature,
+1. Always log the important outputs using advantage kit logging feature, unless the output is already autologged.
+2. do not add logging to the console, unless prompted by the user to debug a code
+2. Always add the numbers that may need changing by the user to RobotConstants.java as TunableNumbers
+</making_code_changes>
+
+<debugging>
+when asked by user to debug or user is reporting a problem, always add system.out.println() for debug messages.
+</debugging>
+
+<reading_code>
+when reading, understanding and explaining code, Always think with the code's connection with the compitition robot in mind. 
+
+

.vscode/tasks.json

@@ -0,0 +1,119 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "Kill Gradle Processes",
+            "type": "shell",
+            "windows": {
+                "command": "powershell",
+                "args": [
+                    "-Command",
+                    "Get-Process java -ErrorAction SilentlyContinue | Stop-Process -Force"
+                ]
+            },
+            "presentation": {
+                "reveal": "never",
+                "panel": "shared"
+            },
+            "problemMatcher": []
+        },
+        {
+            "label": "Build Robot Code",
+            "type": "shell",
+            "command": "./gradlew",
+            "args": [
+                "build"
+            ],
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            },
+            "problemMatcher": [],
+            "options": {
+                "env": {
+                    "JAVA_HOME": "C:\\Users\\Public\\wpilib\\2025\\jdk"
+                }
+            }
+        },
+        {
+            "label": "Deploy Robot Code",
+            "type": "shell",
+            "command": "./gradlew",
+            "args": [
+                "deploy"
+            ],
+            "group": "build",
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            },
+            "problemMatcher": [],
+            "options": {
+                "env": {
+                    "JAVA_HOME": "C:\\Users\\Public\\wpilib\\2025\\jdk"
+                }
+            }
+        },
+        {
+            "label": "Clean Build",
+            "type": "shell",
+            "command": "./gradlew",
+            "args": [
+                "clean",
+                "build"
+            ],
+            "group": "build",
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            },
+            "problemMatcher": [],
+            "options": {
+                "env": {
+                    "JAVA_HOME": "C:\\Users\\Public\\wpilib\\2025\\jdk"
+                }
+            }
+        },
+        {
+            "label": "Run Simulation",
+            "type": "shell",
+            "command": "./gradlew",
+            "args": [
+                "simulateJava"
+            ],
+            "group": "build",
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            },
+            "problemMatcher": [],
+            "options": {
+                "env": {
+                    "JAVA_HOME": "C:\\Users\\Public\\wpilib\\2025\\jdk"
+                }
+            }
+        },
+        {
+            "label": "Deploy to Simulation",
+            "dependsOn": [
+                "Kill Gradle Processes",
+                "Run Simulation"
+            ],
+            "dependsOrder": "sequence",
+            "group": "build",
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            },
+            "options": {
+                "env": {
+                    "JAVA_HOME": "C:\\Users\\Public\\wpilib\\2025\\jdk"
+                }
+            }
+        }
+    ]
+} 

src/main/java/frc/robot/RobotContainer.java

@@ -1,11 +1,15 @@
 package frc.robot;
 
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ArmSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import static frc.robot.consts.ARM_TARGET_ANGLE;
 
 public class RobotContainer {
   private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
+  private final ArmSubsystem m_armSubsystem = new ArmSubsystem(); // Add ArmSubsystem instance
 
   // Single Xbox controller for driving
   CommandXboxController mainController = new CommandXboxController(0);
@@ -18,7 +22,7 @@ private void configureBindings() {
     // Default drive command: single joystick arcade drive using velocity control
     Command velocityArcadeDrive =
       m_driveSubsystem.run(() -> {
-        double forwardInput = deadBand(-mainController.getLeftY(), 0.1);
+        double forwardInput = deadBand(mainController.getLeftY(), 0.1);
         double turnInput = deadBand(mainController.getLeftX(), 0.1);
 
         // Convert joystick input [-1..1] to RPM [-MAX_DRIVE_RPM..MAX_DRIVE_RPM]
@@ -34,8 +38,14 @@ private void configureBindings() {
 
     m_driveSubsystem.setDefaultCommand(velocityArcadeDrive);
 
-    // Optionally bind other commands here, e.g.:
-    // mainController.a().toggleOnTrue(new SomeOtherCommand(m_driveSubsystem));
+    // Bind the "A" button to set the current position as zero (reset encoder)
+    mainController.a().onTrue(new InstantCommand(() -> m_armSubsystem.resetArmPositionToZero()));
+
+    // Bind the "X" button to move the arm to the target position (tunable)
+    mainController.x().onTrue(new InstantCommand(() -> m_armSubsystem.setArmPosition(consts.ARM_TARGET_ANGLE.get())));
+
+    // Bind the "B" button to move the arm back to zero
+    mainController.b().onTrue(new InstantCommand(() -> m_armSubsystem.setArmPosition(0)));
   }
 
   // Deadband helper to avoid drift

src/main/java/frc/robot/consts.java

@@ -1,5 +1,8 @@
 package frc.robot;
 
+import frc.robot.utils.TunableNumber;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 /* const naming standards
  * nameName + c (const) + type
  */
@@ -9,29 +12,19 @@ public final class consts {
   public static final class CANID {
     public static final int LCanIDci = 0;
     public static final int RCanIDci = 1;
+    public static final int armCanIDci = 2;
   }
+  public static final boolean TUNING = true;
 
-  // Velocity PID
-  public static final class VelPID {
-    public static final double velKPcd = 0.01;
-    public static final double velKIcd = 0.0;
-    public static final double velKDcd = 0.0;
-    public static final double velKVcd = 0.12;
-  }
-
-  // Position PID
-  public static final class PosPID {
-    public static final double posKPcd = 1.5;
-    public static final double posKIcd = 0.0;
-    public static final double posKDcd = 0.05;
-  }
 
   // Max values
   public static final class Maximums {
-    // TODO: This is an example. Value is untested
-    // This value controls the maximum of which the drive motors can run in
-    // Real drive RPM = proportion * maxDriveRPMcd, proportion in [0, 1]
-    public static final double maxDriveRPMcd = 1000.0;
+    public static final double maxDriveRPMcd = 5000.0; // Example value
   }
-
+  public static final TunableNumber ARM_TARGET_ANGLE = new TunableNumber("Arm Target Angle", 20.0);
+
+  // Arm PID constants
+  public static final TunableNumber ARM_KP = new TunableNumber("Arm kP", 0.2);
+  public static final TunableNumber ARM_KI = new TunableNumber("Arm kI", 0.0);
+  public static final TunableNumber ARM_KD = new TunableNumber("Arm kD", 0.0);
 }

src/main/java/frc/robot/subsystems/ArmSubsystem.java

@@ -1,5 +1,67 @@
 package frc.robot.subsystems;
+import frc.robot.consts;
 
-public class ArmSubsystem {
-
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+import static frc.robot.consts.ARM_KP;
+import static frc.robot.consts.ARM_KI;
+import static frc.robot.consts.ARM_KD;
+
+public class ArmSubsystem extends SubsystemBase {
+    private final TalonFX motorArm = new TalonFX(consts.CANID.armCanIDci);
+    public double armAngle; // Current angle of the arm in degrees
+    private double targetPosition = 0;
+    private double lastKP = ARM_KP.get();
+    private double lastKI = ARM_KI.get();
+    private double lastKD = ARM_KD.get();
+    public ArmSubsystem() {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        config.withSlot0(new Slot0Configs().withKP(ARM_KP.get()).withKI(ARM_KI.get()).withKD(ARM_KD.get()));
+        motorArm.getConfigurator().apply(config);
+    }
+
+    /**
+     * Resets the current position of the arm to 0° (sets encoder to zero).
+     */
+    public void resetArmPositionToZero() {
+        motorArm.setPosition(0);
+        Logger.recordOutput("Arm/EncoderZeroed", true);
+    }
+
+    /**
+     * Sets the arm position to a desired angle.
+     * @param targetAngle The desired angle in degrees.
+     */
+    public void setArmPosition(double targetAngle) {
+        targetPosition = targetAngle;
+        motorArm.setControl(new PositionDutyCycle(targetAngle));
+    }
+
+    @Override
+    public void periodic() {
+        // Update PID if changed
+        if (ARM_KP.get() != lastKP || ARM_KI.get() != lastKI || ARM_KD.get() != lastKD) {
+            Slot0Configs slot0 = new Slot0Configs()
+                .withKP(ARM_KP.get())
+                .withKI(ARM_KI.get())
+                .withKD(ARM_KD.get());
+            TalonFXConfiguration config = new TalonFXConfiguration();
+            config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+            config.withSlot0(slot0);
+            motorArm.getConfigurator().apply(config);
+            lastKP = ARM_KP.get();
+            lastKI = ARM_KI.get();
+            lastKD = ARM_KD.get();
+        }
+        // Log the current arm position every cycle
+        Logger.recordOutput("Arm/Position", motorArm.getPosition().getValue());
+        Logger.recordOutput("Arm/TargetAngle", targetPosition);
+    }
 }