diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index c127537..06f324c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2026", - "teamNumber": 9930 + "teamNumber": 9990 } \ No newline at end of file diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index a84f77f..af9cf59 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -9,11 +9,11 @@ "sidebar": { "width": 300, "expanded": [ - "/Drive", - "/RealOutputs", - "/RealOutputs/SwerveStates", - "/RealOutputs/SwerveChassisSpeeds", - "/RealOutputs/Odometry" + "NT:/AdvantageKit/Drive", + "NT:/AdvantageKit/RealOutputs", + "NT:/AdvantageKit/RealOutputs/SwerveStates", + "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds", + "NT:/AdvantageKit/RealOutputs/Odometry" ] }, "tabs": { @@ -34,7 +34,7 @@ "sources": [ { "type": "states", - "logKey": "/RealOutputs/SwerveStates/Measured", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", "logType": "SwerveModuleState[]", "visible": true, "options": { @@ -44,7 +44,7 @@ }, { "type": "states", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized", "logType": "SwerveModuleState[]", "visible": true, "options": { @@ -54,7 +54,7 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured", "logType": "ChassisSpeeds", "visible": true, "options": { @@ -63,7 +63,7 @@ }, { "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints", "logType": "ChassisSpeeds", "visible": true, "options": { @@ -72,7 +72,7 @@ }, { "type": "rotation", - "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation", "logType": "Rotation2d", "visible": true, "options": {} @@ -94,7 +94,7 @@ "sources": [ { "type": "robot", - "logKey": "/RealOutputs/Odometry/Robot", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", "logType": "Pose2d", "visible": true, "options": { @@ -103,7 +103,7 @@ }, { "type": "trajectory", - "logKey": "/RealOutputs/Odometry/Trajectory", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Trajectory", "logType": "Pose2d[]", "visible": true, "options": { @@ -113,7 +113,7 @@ }, { "type": "ghost", - "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/TrajectorySetpoint", "logType": "Pose2d", "visible": true, "options": { @@ -148,7 +148,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module0/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -158,7 +158,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module1/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module1/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -168,7 +168,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module2/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module2/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -178,7 +178,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module3/TurnPosition/value", + "logKey": "NT:/AdvantageKit/Drive/Module3/TurnPosition/value", "logType": "Number", "visible": true, "options": { @@ -213,7 +213,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module0/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -223,7 +223,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module1/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module1/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -233,7 +233,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module2/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module2/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -243,7 +243,7 @@ }, { "type": "stepped", - "logKey": "/Drive/Module3/DrivePositionRad", + "logKey": "NT:/AdvantageKit/Drive/Module3/DrivePositionRad", "logType": "Number", "visible": true, "options": { @@ -278,7 +278,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/angle/value", "logType": "Number", "visible": true, "options": { @@ -288,7 +288,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", "logType": "Number", "visible": true, "options": { @@ -323,7 +323,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/speed", "logType": "Number", "visible": true, "options": { @@ -333,7 +333,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", "logType": "Number", "visible": true, "options": { @@ -368,7 +368,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured/vx", "logType": "Number", "visible": true, "options": { @@ -378,7 +378,7 @@ }, { "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured/vy", "logType": "Number", "visible": true, "options": { @@ -413,7 +413,7 @@ "leftSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DriveCurrentAmps", + "logKey": "NT:/AdvantageKit/Drive/Module0/DriveCurrentAmps", "logType": "Number", "visible": true, "options": { @@ -425,7 +425,7 @@ "rightSources": [ { "type": "stepped", - "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logKey": "NT:/AdvantageKit/Drive/Module0/DriveVelocityRadPerSec", "logType": "Number", "visible": true, "options": { diff --git a/src/main/deploy/pathplanner/paths/Center.0.path b/src/main/deploy/pathplanner/paths/Center.0.path index 07aeb9f..60e1418 100644 --- a/src/main/deploy/pathplanner/paths/Center.0.path +++ b/src/main/deploy/pathplanner/paths/Center.0.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center.1.path b/src/main/deploy/pathplanner/paths/Center.1.path index 1013bf4..cf48b96 100644 --- a/src/main/deploy/pathplanner/paths/Center.1.path +++ b/src/main/deploy/pathplanner/paths/Center.1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": -90.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Feed.0.path b/src/main/deploy/pathplanner/paths/Feed.0.path index 3afd57e..ef30e8f 100644 --- a/src/main/deploy/pathplanner/paths/Feed.0.path +++ b/src/main/deploy/pathplanner/paths/Feed.0.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Feed.1.path b/src/main/deploy/pathplanner/paths/Feed.1.path index bb74391..e13c9f9 100644 --- a/src/main/deploy/pathplanner/paths/Feed.1.path +++ b/src/main/deploy/pathplanner/paths/Feed.1.path @@ -65,8 +65,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f981b63..564bf61 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,18 +4,29 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, "robotMass": 74.088, "robotMOI": 6.883, - "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60FOC", + "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc89419..6685fe8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,11 +10,14 @@ import static frc.robot.subsystems.vision.VisionConstants.*; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.goals.RobotGoals; @@ -56,6 +59,7 @@ public class RobotContainer { // Controller private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController characterizeController = new CommandXboxController(4); // Reactive architecture components private final OperatorIntent operatorIntent; @@ -166,6 +170,7 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); + configureCharacterizationButtonBindings(); } /** @@ -208,6 +213,49 @@ private void configureButtonBindings() { // .ignoringDisable(true)); } + public void configureCharacterizationButtonBindings() { + characterizeController + .back() + .and(characterizeController.y()) + .whileTrue(drive.sysIdDynamic(Direction.kForward)); + characterizeController + .back() + .and(characterizeController.x()) + .whileTrue(drive.sysIdDynamic(Direction.kReverse)); + characterizeController + .start() + .and(characterizeController.y()) + .whileTrue(drive.sysIdQuasistatic(Direction.kForward)); + characterizeController + .start() + .and(characterizeController.x()) + .whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); + + characterizeController + .povUp() + .whileTrue(DriveCommands.wheelRadiusCharacterization(drive)) + .onFalse(DriveCommands.brakeDrive(drive)); + + characterizeController + .a() + .onTrue( + new InstantCommand( + () -> { + SignalLogger.setPath("/media/sda1/logs"); + // SignalLogger.enableAutoLogging(true); + SignalLogger.start(); + System.out.println("Started Logger"); + })); + characterizeController + .b() + .onTrue( + new InstantCommand( + () -> { + SignalLogger.stop(); + System.out.println("Stopped Logger"); + })); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a749f60..578bfb8 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.drive.Drive; import java.text.DecimalFormat; import java.text.NumberFormat; @@ -287,4 +288,11 @@ private static class WheelRadiusCharacterizationState { Rotation2d lastAngle = Rotation2d.kZero; double gyroDelta = 0.0; } + + public static Command brakeDrive(Drive drive) { + return new InstantCommand( + () -> { + drive.stop(); + }); + } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index d10ebae..9542cee 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -32,8 +32,14 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - + // new Slot0Configs() + // .withKP(0.49) // 0.2204171275 + // .withKI(0) + // .withKD(0.0) + // .withKS(0.125) // 0.155625 + // .withKV(0.734) // 0.7290475 + // .withKA(0.103); // 0.08876375; + new Slot0Configs().withKP(0.34).withKI(0).withKD(0).withKS(0.22).withKV(0.72).withKA(0.06); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; @@ -54,7 +60,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); + private static final Current kSlipCurrent = Amps.of(46); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -78,15 +84,20 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.24); + // Max theroretical speed 5.4 + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.81); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 0; private static final double kDriveGearRatio = 5.8909090909090915; - private static final double kSteerGearRatio = 11.314285714285717; - private static final Distance kWheelRadius = Inches.of(2); + private static final double kSteerGearRatio = 12.1; + // ********** Wheel Radius Characterization Results ********** + // Wheel Delta: 55.224 radians + // Gyro Delta: 6.245 radians + // Wheel Radius: 0.049 meters, 1.943 inches + private static final Distance kWheelRadius = Inches.of(1.943); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; diff --git a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java index 61cc4ca..322b824 100644 --- a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java +++ b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java @@ -25,7 +25,7 @@ public RobotGoalsBehavior(RobotGoals goals) { public void configure(OperatorIntentEvents intent) { intent .wantsToScoreTrigger() - .whileTrue(goals.setGoalCommand(RobotGoal.LAUNCHING)) - .whileFalse(goals.setGoalCommand(RobotGoal.IDLE)); + .onTrue(goals.setGoalCommand(RobotGoal.LAUNCHING)) + .onFalse(goals.setGoalCommand(RobotGoal.IDLE)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java index cc7999d..223664c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java @@ -53,16 +53,15 @@ public void setIndexerSpeed(Voltage speed) { public Command launchCommand() { return runOnce( () -> { - setIndexerSpeed(Volts.of(2)); - setLaunchSpeed(Volts.of(2)); + setIndexerSpeed(Volts.of(4.0)); + setLaunchSpeed(Volts.of(11.0)); }); } public Command idleCommand() { return runOnce( () -> { - setIndexerSpeed(Volts.of(0)); - setLaunchSpeed(Volts.of(0)); + stop(); }); }