diff --git a/.cursor/rules/general.mdc b/.cursor/rules/general.mdc
new file mode 100644
index 0000000..08012b4
--- /dev/null
+++ b/.cursor/rules/general.mdc
@@ -0,0 +1,12 @@
+---
+description:
+globs:
+alwaysApply: true
+---
+
+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.
+
\ No newline at end of file
diff --git a/.cursor/rules/wpi-frc.mdc b/.cursor/rules/wpi-frc.mdc
new file mode 100644
index 0000000..20b5faf
--- /dev/null
+++ b/.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
+
+
+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
+
+
+
+when asked by user to debug or user is reporting a problem, always add system.out.println() for debug messages.
+
+
+
+when reading, understanding and explaining code, Always think with the code's connection with the compitition robot in mind.
+
+
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
new file mode 100644
index 0000000..0b22276
--- /dev/null
+++ b/.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"
+ }
+ }
+ }
+ ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 30cc1f2..b82340a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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
diff --git a/src/main/java/frc/robot/consts.java b/src/main/java/frc/robot/consts.java
index 7d34701..25f4d0f 100644
--- a/src/main/java/frc/robot/consts.java
+++ b/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);
}
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index caa3626..7fed353 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/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);
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 400d74e..7afcc0d 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import frc.robot.consts;
+import frc.robot.utils.TunableNumber;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.configs.Slot0Configs;
@@ -19,11 +20,16 @@
public class DriveSubsystem extends SubsystemBase {
private final TalonFX motorFrontRight = new TalonFX(consts.CANID.RCanIDci);
private final TalonFX motorFrontLeft = new TalonFX(consts.CANID.LCanIDci);
- private final PIDController pid = new PIDController(consts.PosPID.posKPcd, consts.PosPID.posKIcd, consts.PosPID.posKDcd);
+ private final PIDController pid = new PIDController(consts.VelPID.DRIVE_VELOCITY_KP.get(), consts.VelPID.DRIVE_VELOCITY_KI.get(), consts.VelPID.DRIVE_VELOCITY_KD.get());
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
// private final PositionDutyCycle positionRequest = new PositionDutyCycle(0);
+ // Tunable numbers for PID constants
+ private final TunableNumber kP = new TunableNumber("DriveSubsystem/kP", 0.1);
+ private final TunableNumber kI = new TunableNumber("DriveSubsystem/kI", 0.0);
+ private final TunableNumber kD = new TunableNumber("DriveSubsystem/kD", 0.0);
+
/** Enum to track which PID mode is active */
private enum DriveMode {
VELOCITY, POSITION
@@ -32,25 +38,31 @@ private enum DriveMode {
private DriveMode currentMode = null;
public DriveSubsystem() {
+ applyMotorConfigs(); // Apply configurations during initialization
+ }
+
+ /**
+ * Helper method to apply configurations to motors.
+ */
+ private void applyMotorConfigs() {
motorFrontLeft.getConfigurator().apply(genConfig(true));
motorFrontRight.getConfigurator().apply(genConfig(false));
}
- public TalonFXConfiguration genConfig(boolean inverted) {
+ /**
+ * Generates a configuration for the motors.
+ * @param isLeftMotor Whether the configuration is for the left motor.
+ * @return The generated configuration.
+ */
+ private TalonFXConfiguration genConfig(boolean isLeftMotor) {
TalonFXConfiguration config = new TalonFXConfiguration();
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
- if (inverted) {
+ if (isLeftMotor) {
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
} else {
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
}
- Slot0Configs slot0 = config.Slot0;
- slot0.kP = consts.VelPID.velKPcd;
- slot0.kI = consts.VelPID.velKIcd;
- slot0.kD = consts.VelPID.velKDcd;
- slot0.kV = consts.VelPID.velKVcd;
-
return config;
}
@@ -84,6 +96,8 @@ public void periodic() {
// Log current control mode
Logger.recordOutput("Drive/Mode", currentMode == null ? "NONE" : currentMode.name());
+
+
// LEFT motor logs
Logger.recordOutput("Drive/Left/Temp", motorFrontLeft.getDeviceTemp().getValue());
Logger.recordOutput("Drive/Left/VelocityRPM", motorFrontLeft.getVelocity().getValue());
diff --git a/src/main/java/frc/robot/utils/TunableNumber.java b/src/main/java/frc/robot/utils/TunableNumber.java
new file mode 100644
index 0000000..7785631
--- /dev/null
+++ b/src/main/java/frc/robot/utils/TunableNumber.java
@@ -0,0 +1,87 @@
+package frc.robot.utils;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.consts;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns
+ * default if not or
+ * value not in dashboard.
+ */
+public class TunableNumber {
+ private static final String tableKey = "TunableNumbers";
+
+ private final String key;
+ private double defaultValue;
+ private double lastHasChangedValue = defaultValue;
+
+ /**
+ * Create a new TunableNumber
+ *
+ * @param dashboardKey Key on dashboard
+ */
+ public TunableNumber(String dashboardKey) {
+ this.key = tableKey + "/" + dashboardKey;
+ }
+
+ /**
+ * Create a new TunableNumber with the default value
+ *
+ * @param dashboardKey Key on dashboard
+ * @param defaultValue Default value
+ */
+ public TunableNumber(String dashboardKey, double defaultValue) {
+ this(dashboardKey);
+ setDefault(defaultValue);
+ }
+
+ /**
+ * Get the default value for the number that has been set
+ *
+ * @return The default value
+ */
+ public double getDefault() {
+ return defaultValue;
+ }
+
+ /**
+ * Set the default value of the number
+ *
+ * @param defaultValue The default value
+ */
+ public void setDefault(double defaultValue) {
+ this.defaultValue = defaultValue;
+ if (consts.TUNING) {
+ // This makes sure the data is on NetworkTables but will not change it
+ SmartDashboard.putNumber(key,
+ SmartDashboard.getNumber(key, defaultValue));
+ }
+ }
+
+ /**
+ * Get the current value, from dashboard if available and in tuning mode
+ *
+ * @return The current value
+ */
+ public double get() {
+ return consts.TUNING ? SmartDashboard.getNumber(key, defaultValue)
+ : defaultValue;
+ }
+
+ /**
+ * Checks whether the number has changed since our last check
+ *
+ * @return True if the number has changed since the last time this method was
+ * called, false
+ * otherwise
+ */
+ public boolean hasChanged() {
+ double currentValue = get();
+ if (currentValue != lastHasChangedValue) {
+ lastHasChangedValue = currentValue;
+ return true;
+ }
+
+ return false;
+ }
+}
\ No newline at end of file