From 6375f35e2a8c6d42125f4cbc7b9d021587acf0cf Mon Sep 17 00:00:00 2001
From: Dale <13660425055@163.com>
Date: Wed, 2 Jul 2025 14:47:34 +0800
Subject: [PATCH 1/5] 1
---
src/main/java/frc/robot/RobotContainer.java | 14 ++-
src/main/java/frc/robot/consts.java | 6 +-
.../frc/robot/subsystems/ArmSubsystem.java | 38 +++++++-
.../frc/robot/subsystems/DriveSubsystem.java | 17 +++-
.../java/frc/robot/utils/TunableNumber.java | 87 +++++++++++++++++++
5 files changed, 154 insertions(+), 8 deletions(-)
create mode 100644 src/main/java/frc/robot/utils/TunableNumber.java
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 30cc1f2..68a484a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,11 +1,14 @@
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;
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);
@@ -34,8 +37,15 @@ 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 reset the arm position
+ mainController.a().onTrue(new InstantCommand(() -> m_armSubsystem.resetArmPosition()));
+
+ // Bind the "X" button to set the arm position to a desired angle (e.g., 45 degrees),
+ // then move the arm back to the previously set armAngle
+ mainController.x().onTrue(new InstantCommand(() -> {
+ m_armSubsystem.setArmPosition(45.0); // Move to the desired angle
+ m_armSubsystem.resetArmPositionToZero(); // Reset the motor position to 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..98d7b07 100644
--- a/src/main/java/frc/robot/consts.java
+++ b/src/main/java/frc/robot/consts.java
@@ -1,5 +1,7 @@
package frc.robot;
+import frc.robot.utils.TunableNumber;
+
/* const naming standards
* nameName + c (const) + type
*/
@@ -9,11 +11,13 @@ 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 TunableNumber DRIVE_VELOCITY_KP = new TunableNumber("00",0);
public static final double velKIcd = 0.0;
public static final double velKDcd = 0.0;
public static final double velKVcd = 0.12;
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index caa3626..0a6f090 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -1,5 +1,39 @@
package frc.robot.subsystems;
+import frc.robot.consts;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
-public class ArmSubsystem {
-
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ArmSubsystem extends SubsystemBase {
+ private final TalonFX motorArm = new TalonFX(consts.CANID.armCanIDci);
+ public Angle armAngle; // Current angle of the arm in degrees
+
+ public ArmSubsystem() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ motorArm.getConfigurator().apply(config);
+ }
+
+ /**
+ * Resets the current position of the arm to 0°.
+ */
+ public void resetArmPosition() {
+ // Reset the encoder position to zero
+ armAngle = motorArm.getPosition().getValue(); // Extract angle in degrees
+ }
+ public void resetArmPositionToZero() {
+ motorArm.setPosition(armAngle); // Reset the motor position
+ }
+ /**
+ * Sets the arm position to a desired angle.
+ * @param targetAngle The desired angle in degrees.
+ */
+ public void setArmPosition(double targetAngle) {
+ // Convert the target angle to motor position units (if necessary)
+ double targetPosition = targetAngle; // Assuming 1 degree corresponds to 1 unit
+ motorArm.setPosition(targetPosition); // Set the motor position
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 400d74e..301c415 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;
@@ -24,6 +25,11 @@ public class DriveSubsystem extends SubsystemBase {
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", consts.VelPID.velKPcd);
+ private final TunableNumber kI = new TunableNumber("DriveSubsystem/kI", consts.VelPID.velKIcd);
+ private final TunableNumber kD = new TunableNumber("DriveSubsystem/kD", consts.VelPID.velKDcd);
+
/** Enum to track which PID mode is active */
private enum DriveMode {
VELOCITY, POSITION
@@ -46,9 +52,10 @@ public TalonFXConfiguration genConfig(boolean inverted) {
}
Slot0Configs slot0 = config.Slot0;
- slot0.kP = consts.VelPID.velKPcd;
- slot0.kI = consts.VelPID.velKIcd;
- slot0.kD = consts.VelPID.velKDcd;
+ // Retrieve PID values from TunableNumber instances
+ slot0.kP = kP.get();
+ slot0.kI = kI.get();
+ slot0.kD = kD.get();
slot0.kV = consts.VelPID.velKVcd;
return config;
@@ -84,6 +91,10 @@ public void periodic() {
// Log current control mode
Logger.recordOutput("Drive/Mode", currentMode == null ? "NONE" : currentMode.name());
+ // Update motor configurations periodically to reflect changes in PID values
+ motorFrontLeft.getConfigurator().apply(genConfig(true));
+ motorFrontRight.getConfigurator().apply(genConfig(false));
+
// 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
From 8aebbb03effb3da7a1e03553fb63e4c7eec238b8 Mon Sep 17 00:00:00 2001
From: Dale <13660425055@163.com>
Date: Thu, 3 Jul 2025 15:32:24 +0800
Subject: [PATCH 2/5] 1
---
.cursor/rules/general.mdc | 12 ++
.cursor/rules/wpi-frc.mdc | 43 +++++++
.vscode/tasks.json | 119 ++++++++++++++++++
src/main/java/frc/robot/RobotContainer.java | 18 +--
src/main/java/frc/robot/consts.java | 25 ++--
.../frc/robot/subsystems/ArmSubsystem.java | 24 ++--
.../frc/robot/subsystems/DriveSubsystem.java | 30 +++--
7 files changed, 227 insertions(+), 44 deletions(-)
create mode 100644 .cursor/rules/general.mdc
create mode 100644 .cursor/rules/wpi-frc.mdc
create mode 100644 .vscode/tasks.json
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 68a484a..751e35d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -5,6 +5,7 @@
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();
@@ -21,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]
@@ -37,15 +38,14 @@ private void configureBindings() {
m_driveSubsystem.setDefaultCommand(velocityArcadeDrive);
- // Bind the "A" button to reset the arm position
- mainController.a().onTrue(new InstantCommand(() -> m_armSubsystem.resetArmPosition()));
+ // 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 set the arm position to a desired angle (e.g., 45 degrees),
- // then move the arm back to the previously set armAngle
- mainController.x().onTrue(new InstantCommand(() -> {
- m_armSubsystem.setArmPosition(45.0); // Move to the desired angle
- m_armSubsystem.resetArmPositionToZero(); // Reset the motor position to 0
- }));
+ // Bind the "X" button to move the arm to the target position (tunable)
+ mainController.x().onTrue(new InstantCommand(() -> m_armSubsystem.setArmPosition(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 98d7b07..f0f2e1d 100644
--- a/src/main/java/frc/robot/consts.java
+++ b/src/main/java/frc/robot/consts.java
@@ -1,6 +1,7 @@
package frc.robot;
import frc.robot.utils.TunableNumber;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/* const naming standards
* nameName + c (const) + type
@@ -17,25 +18,17 @@ public static final class CANID {
// Velocity PID
public static final class VelPID {
- public static final TunableNumber DRIVE_VELOCITY_KP = new TunableNumber("00",0);
- 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;
+ public static final TunableNumber DRIVE_VELOCITY_KP = new TunableNumber("Drive Velocity kP", 0.0);
+ public static final TunableNumber DRIVE_VELOCITY_KI = new TunableNumber("Drive Velocity kI", 0.0);
+ public static final TunableNumber DRIVE_VELOCITY_KD = new TunableNumber("Drive Velocity kD", 0.0);
+ public static final TunableNumber DRIVE_VELOCITY_KV = new TunableNumber("Drive Velocity kV", 0.12);
}
// 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
}
-
+
+ // Arm target angle
+ public static final TunableNumber ARM_TARGET_ANGLE = new TunableNumber("Arm Target Angle", 30.0);
}
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 0a6f090..56c181f 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -6,10 +6,11 @@
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
public class ArmSubsystem extends SubsystemBase {
private final TalonFX motorArm = new TalonFX(consts.CANID.armCanIDci);
- public Angle armAngle; // Current angle of the arm in degrees
+ public double armAngle; // Current angle of the arm in degrees
public ArmSubsystem() {
TalonFXConfiguration config = new TalonFXConfiguration();
@@ -18,22 +19,27 @@ public ArmSubsystem() {
}
/**
- * Resets the current position of the arm to 0°.
+ * Resets the current position of the arm to 0° (sets encoder to zero).
*/
- public void resetArmPosition() {
- // Reset the encoder position to zero
- armAngle = motorArm.getPosition().getValue(); // Extract angle in degrees
- }
public void resetArmPositionToZero() {
- motorArm.setPosition(armAngle); // Reset the motor position
+ 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) {
// Convert the target angle to motor position units (if necessary)
- double targetPosition = targetAngle; // Assuming 1 degree corresponds to 1 unit
- motorArm.setPosition(targetPosition); // Set the motor position
+ double targetPosition = targetAngle; // Assuming 1 degree = 1 unit
+ motorArm.setPosition(targetPosition);
+ Logger.recordOutput("Arm/TargetAngle", targetAngle);
+ }
+
+ @Override
+ public void periodic() {
+ // Log the current arm position every cycle
+ Logger.recordOutput("Arm/Position", motorArm.getPosition().getValue());
}
}
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 301c415..75f0465 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -20,15 +20,15 @@
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", consts.VelPID.velKPcd);
- private final TunableNumber kI = new TunableNumber("DriveSubsystem/kI", consts.VelPID.velKIcd);
- private final TunableNumber kD = new TunableNumber("DriveSubsystem/kD", consts.VelPID.velKDcd);
+ private final TunableNumber kP = new TunableNumber("DriveSubsystem/kP", consts.VelPID.DRIVE_VELOCITY_KP.get());
+ private final TunableNumber kI = new TunableNumber("DriveSubsystem/kI", consts.VelPID.DRIVE_VELOCITY_KI.get());
+ private final TunableNumber kD = new TunableNumber("DriveSubsystem/kD", consts.VelPID.DRIVE_VELOCITY_KD.get());
/** Enum to track which PID mode is active */
private enum DriveMode {
@@ -38,14 +38,26 @@ 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;
@@ -56,7 +68,7 @@ public TalonFXConfiguration genConfig(boolean inverted) {
slot0.kP = kP.get();
slot0.kI = kI.get();
slot0.kD = kD.get();
- slot0.kV = consts.VelPID.velKVcd;
+ slot0.kV = consts.VelPID.DRIVE_VELOCITY_KV.get();
return config;
}
@@ -91,9 +103,7 @@ public void periodic() {
// Log current control mode
Logger.recordOutput("Drive/Mode", currentMode == null ? "NONE" : currentMode.name());
- // Update motor configurations periodically to reflect changes in PID values
- motorFrontLeft.getConfigurator().apply(genConfig(true));
- motorFrontRight.getConfigurator().apply(genConfig(false));
+
// LEFT motor logs
Logger.recordOutput("Drive/Left/Temp", motorFrontLeft.getDeviceTemp().getValue());
From 5ab4f5089c5518572b8289d77b46d4b082a55ee6 Mon Sep 17 00:00:00 2001
From: Dale <13660425055@163.com>
Date: Thu, 3 Jul 2025 15:54:12 +0800
Subject: [PATCH 3/5] 1
---
src/main/java/frc/robot/RobotContainer.java | 2 +-
.../java/frc/robot/subsystems/ArmSubsystem.java | 14 ++++++++------
2 files changed, 9 insertions(+), 7 deletions(-)
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 751e35d..a0bf126 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -42,7 +42,7 @@ private void configureBindings() {
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(ARM_TARGET_ANGLE.get())));
+ mainController.x().onTrue(new InstantCommand(() -> m_armSubsystem.setArmPosition(30)));
// Bind the "B" button to move the arm back to zero
mainController.b().onTrue(new InstantCommand(() -> m_armSubsystem.setArmPosition(0)));
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 56c181f..fe1c514 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -1,9 +1,11 @@
package frc.robot.subsystems;
import frc.robot.consts;
+
+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.VoltageOut;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
@@ -11,10 +13,11 @@
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;
public ArmSubsystem() {
TalonFXConfiguration config = new TalonFXConfiguration();
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ config.withSlot0(new Slot0Configs().withKP(1).withKI(0).withKD(0));
motorArm.getConfigurator().apply(config);
}
@@ -31,15 +34,14 @@ public void resetArmPositionToZero() {
* @param targetAngle The desired angle in degrees.
*/
public void setArmPosition(double targetAngle) {
- // Convert the target angle to motor position units (if necessary)
- double targetPosition = targetAngle; // Assuming 1 degree = 1 unit
- motorArm.setPosition(targetPosition);
- Logger.recordOutput("Arm/TargetAngle", targetAngle);
+ targetPosition = targetAngle;
+ motorArm.setControl(new VoltageOut(1));
}
@Override
public void periodic() {
// Log the current arm position every cycle
Logger.recordOutput("Arm/Position", motorArm.getPosition().getValue());
+ Logger.recordOutput("Arm/TargetAngle", targetPosition);
}
}
From 58b5657d44e7387f1998826a70f9d2cd31bb675c Mon Sep 17 00:00:00 2001
From: Dale <13660425055@163.com>
Date: Thu, 3 Jul 2025 16:40:54 +0800
Subject: [PATCH 4/5] 1
---
src/main/java/frc/robot/RobotContainer.java | 2 +-
src/main/java/frc/robot/consts.java | 7 +++--
.../frc/robot/subsystems/ArmSubsystem.java | 26 ++++++++++++++++---
.../frc/robot/subsystems/DriveSubsystem.java | 7 -----
4 files changed, 29 insertions(+), 13 deletions(-)
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a0bf126..b82340a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -42,7 +42,7 @@ private void configureBindings() {
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(30)));
+ 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)));
diff --git a/src/main/java/frc/robot/consts.java b/src/main/java/frc/robot/consts.java
index f0f2e1d..eb6e32f 100644
--- a/src/main/java/frc/robot/consts.java
+++ b/src/main/java/frc/robot/consts.java
@@ -28,7 +28,10 @@ public static final class VelPID {
public static final class Maximums {
public static final double maxDriveRPMcd = 5000.0; // Example value
}
-
- // Arm target angle
public static final TunableNumber ARM_TARGET_ANGLE = new TunableNumber("Arm Target Angle", 30.0);
+
+ // Arm PID constants
+ public static final TunableNumber ARM_KP = new TunableNumber("Arm kP", 1.0);
+ 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 fe1c514..7fed353 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -5,19 +5,25 @@
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.controls.VoltageOut;
+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(1).withKI(0).withKD(0));
+ config.withSlot0(new Slot0Configs().withKP(ARM_KP.get()).withKI(ARM_KI.get()).withKD(ARM_KD.get()));
motorArm.getConfigurator().apply(config);
}
@@ -35,11 +41,25 @@ public void resetArmPositionToZero() {
*/
public void setArmPosition(double targetAngle) {
targetPosition = targetAngle;
- motorArm.setControl(new VoltageOut(1));
+ 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 75f0465..5e63625 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -63,13 +63,6 @@ private TalonFXConfiguration genConfig(boolean isLeftMotor) {
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
}
- Slot0Configs slot0 = config.Slot0;
- // Retrieve PID values from TunableNumber instances
- slot0.kP = kP.get();
- slot0.kI = kI.get();
- slot0.kD = kD.get();
- slot0.kV = consts.VelPID.DRIVE_VELOCITY_KV.get();
-
return config;
}
From 4843606f6ba4b4812ea0590c53fbde8d5f9bca4c Mon Sep 17 00:00:00 2001
From: Dale <13660425055@163.com>
Date: Thu, 3 Jul 2025 17:29:02 +0800
Subject: [PATCH 5/5] 1
---
src/main/java/frc/robot/consts.java | 11 ++---------
.../java/frc/robot/subsystems/DriveSubsystem.java | 6 +++---
2 files changed, 5 insertions(+), 12 deletions(-)
diff --git a/src/main/java/frc/robot/consts.java b/src/main/java/frc/robot/consts.java
index eb6e32f..25f4d0f 100644
--- a/src/main/java/frc/robot/consts.java
+++ b/src/main/java/frc/robot/consts.java
@@ -16,22 +16,15 @@ public static final class CANID {
}
public static final boolean TUNING = true;
- // Velocity PID
- public static final class VelPID {
- public static final TunableNumber DRIVE_VELOCITY_KP = new TunableNumber("Drive Velocity kP", 0.0);
- public static final TunableNumber DRIVE_VELOCITY_KI = new TunableNumber("Drive Velocity kI", 0.0);
- public static final TunableNumber DRIVE_VELOCITY_KD = new TunableNumber("Drive Velocity kD", 0.0);
- public static final TunableNumber DRIVE_VELOCITY_KV = new TunableNumber("Drive Velocity kV", 0.12);
- }
// Max values
public static final class Maximums {
public static final double maxDriveRPMcd = 5000.0; // Example value
}
- public static final TunableNumber ARM_TARGET_ANGLE = new TunableNumber("Arm Target Angle", 30.0);
+ 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", 1.0);
+ 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/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 5e63625..7afcc0d 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -26,9 +26,9 @@ public class DriveSubsystem extends SubsystemBase {
// private final PositionDutyCycle positionRequest = new PositionDutyCycle(0);
// Tunable numbers for PID constants
- private final TunableNumber kP = new TunableNumber("DriveSubsystem/kP", consts.VelPID.DRIVE_VELOCITY_KP.get());
- private final TunableNumber kI = new TunableNumber("DriveSubsystem/kI", consts.VelPID.DRIVE_VELOCITY_KI.get());
- private final TunableNumber kD = new TunableNumber("DriveSubsystem/kD", consts.VelPID.DRIVE_VELOCITY_KD.get());
+ 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 {