From 7f85e63b398a9d926968e5b42be16ec51e03e255 Mon Sep 17 00:00:00 2001 From: Ronan-B Date: Tue, 14 Jan 2025 09:29:36 -0800 Subject: [PATCH 01/12] updated pathplanner --- .../java/frc/robot/utils/TrajectoryUtils.java | 76 +++++++++++++++++++ ....1.1.json => PathplannerLib-2025.2.1.json} | 8 +- 2 files changed, 80 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/utils/TrajectoryUtils.java rename vendordeps/{PathplannerLib-2025.1.1.json => PathplannerLib-2025.2.1.json} (87%) diff --git a/src/main/java/frc/robot/utils/TrajectoryUtils.java b/src/main/java/frc/robot/utils/TrajectoryUtils.java new file mode 100644 index 0000000..8143357 --- /dev/null +++ b/src/main/java/frc/robot/utils/TrajectoryUtils.java @@ -0,0 +1,76 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils; + +import frc.robot.constants.SWERVE; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Controls; + +public class TrajectoryUtils { + + public static FollowPathCommand generatePPHolonomicCommand( + CommandSwerveDrivetrain swerveDrive, String pathName, double maxSpeed) { + PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); + + return generatePPHolonomicCommand(swerveDrive, path, maxSpeed, false); + } + + + public static FollowPathHolonomic generatePPHolonomicCommand( + CommandSwerveDrivetrain swerveDrive, String pathName, double maxSpeed, boolean manualFlip) { + PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); + + return generatePPHolonomicCommand(swerveDrive, path, maxSpeed, manualFlip); + } + + public static FollowPathHolonomic generatePPHolonomicCommand( + CommandSwerveDrivetrain swerveDrive, PathPlannerPath path, double maxSpeed) { + + return generatePPHolonomicCommand(swerveDrive, path, maxSpeed, false); + } + + public static FollowPathHolonomic generatePPHolonomicCommand( + CommandSwerveDrivetrain swerveDrive, + PathPlannerPath path, + double maxSpeed, + boolean manualFlip) { + return new FollowPathHolonomic( + path, + () -> swerveDrive.getState().Pose, + swerveDrive::getChassisSpeed, + swerveDrive::setChassisSpeedControlNormal, + new HolonomicPathFollowerConfig( + new PIDConstants(Swerve.kP_X, TunerConstants.kI_X, TunerConstants.kD_X), + new PIDConstants(TunerConstants.kAutoP_Theta, TunerConstants.kAutoI_Theta, TunerConstants.kAutoD_Theta), + maxSpeed, + // 0.898744, + SWERVE.DRIVE.kDriveBaseRadius, + new ReplanningConfig(false, false, 1.0, 0.25)), + () -> manualFlip || Controls.isRedAlliance(), + swerveDrive); + } + + public static FollowPathHolonomic generateStartingPPHolonomicCommand( + CommandSwerveDrivetrain swerveDrive, + PathPlannerPath path, + double maxSpeed, + boolean manualFlip) { + return new FollowPathHolonomic( + path, + () -> swerveDrive.getState().Pose, + swerveDrive::getChassisSpeed, + swerveDrive::setChassisSpeedControlNormal, + new HolonomicPathFollowerConfig( + new PIDConstants(DRIVE.kP_X, DRIVE.kI_X, DRIVE.kD_X), + new PIDConstants(DRIVE.kAutoP_Theta, DRIVE.kAutoI_Theta, DRIVE.kAutoD_Theta), + maxSpeed, + // 0.898744, + SWERVE.DRIVE.kDriveBaseRadius, + new ReplanningConfig(false, false, 1.0, 0.25)), + () -> manualFlip || Controls.isRedAlliance(), + swerveDrive); + } +} diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.1.json index 6322388..71e25f3 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 23403c29a181f564a578faba4d6fdd091f8dfe17 Mon Sep 17 00:00:00 2001 From: Nathan S Date: Tue, 14 Jan 2025 16:40:39 -0800 Subject: [PATCH 02/12] Pretty sure this is the working auto code. --- src/main/java/frc/robot/RobotContainer.java | 99 ++++++++++--------- .../subsystems/CommandSwerveDrivetrain.java | 30 +++--- 2 files changed, 70 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e830bf0..148f299 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,23 +6,29 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.ResetGyro; import frc.robot.commands.RunAlgaeIntake; import frc.robot.commands.RunCoralOuttake; +import frc.robot.commands.SwerveCharacterization; import frc.robot.constants.SWERVE; import frc.robot.constants.USB; +import frc.robot.constants.SWERVE.ROUTINE_TYPE; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.AlgaeIntake; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CoralOuttake; +import frc.robot.utils.SysIdUtils; import frc.robot.utils.Telemetry; /** @@ -41,6 +47,7 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed private final Joystick leftJoystick = new Joystick(USB.leftJoystick); private final SendableChooser m_sysidChooser = new SendableChooser<>(); + private final SendableChooser m_chooser = new SendableChooser<>(); private final Joystick rightJoystick = new Joystick(USB.rightJoystick); private final CommandXboxController m_driverController = new CommandXboxController(USB.xBoxController); @@ -87,8 +94,7 @@ private void initializeSubSystems() { } private void initAutoChooser() { - // Add the autonomous chooser to the dashboard - // SmartDashboard.putData("Auto Mode", m_chooser); + SmartDashboard.putData("Auto Mode", m_chooser); } private void initSmartDashboard() { @@ -96,51 +102,50 @@ private void initSmartDashboard() { // else initAutoChooser(); SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive)); } - - // private void initSysidChooser(){ - // SignalLogger.setPath("/media/sda1/"); - - // SysIdUtils.createSwerveDriveRoutines(m_swerveDrive); - // SysIdUtils.createSwerveTurnRoutines(m_swerveDrive); - - // SmartDashboard.putData( - // "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); - // SmartDashboard.putData( - // "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); - // SmartDashboard.putData( - // "initDriveSettings", - // new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true)); - // SmartDashboard.putData( - // "initTurnSettings",new - // InstantCommand(m_swerveDrive::initTurnSysid).ignoringDisable(true)); - - // m_sysidChooser.addOption( - // "driveQuasistaticForward", - // new SwerveDriveQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kForward)); - // m_sysidChooser.addOption( - // "driveQuasistaticBackwards", - // new SwerveDriveQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kReverse)); - // m_sysidChooser.addOption( - // "driveDynamicForward", - // new SwerveDriveDynamic(m_swerveDrive, SysIdRoutine.Direction.kForward)); - // m_sysidChooser.addOption( - // "driveDynamicBackward", - // new SwerveDriveDynamic(m_swerveDrive, SysIdRoutine.Direction.kReverse)); - - // m_sysidChooser.addOption( - // "turnQuasistaticForward", - // new SwerveTurnQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kForward)); - // m_sysidChooser.addOption( - // "turnQuasistaticBackwards", - // new SwerveTurnQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kReverse)); - // m_sysidChooser.addOption( - // "turnDynamicForward", - // new SwerveTurnDynamic(m_swerveDrive, SysIdRoutine.Direction.kForward)); - // m_sysidChooser.addOption( - // "turnDynamicBackward", - // new SwerveTurnDynamic(m_swerveDrive, SysIdRoutine.Direction.kReverse)); - - // } + + private void initSysidChooser(){ + SignalLogger.setPath("/media/sda1/"); + + SysIdUtils.createSwerveDriveRoutines(m_swerveDrive); + SysIdUtils.createSwerveTurnRoutines(m_swerveDrive); + + SmartDashboard.putData( + "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); + SmartDashboard.putData( + "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); + SmartDashboard.putData( + "initDriveSettings", + new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true)); + SmartDashboard.putData( + "initTurnSettings",new InstantCommand(m_swerveDrive::initTurnSysid).ignoringDisable(true)); + + m_sysidChooser.addOption( + "driveQuasistaticForward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + m_sysidChooser.addOption( + "driveQuasistaticBackwards", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + m_sysidChooser.addOption( + "driveDynamicForward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC)); + m_sysidChooser.addOption( + "driveDynamicBackward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC)); + + m_sysidChooser.addOption( + "turnQuasistaticForward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC)); + m_sysidChooser.addOption( + "turnQuasistaticBackwards", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC)); + m_sysidChooser.addOption( + "turnDynamicForward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC)); + m_sysidChooser.addOption( + "turnDynamicBackward", + new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC)); + + } /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index eff2986..2213010 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,8 +2,11 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -25,7 +28,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.utils.ModuleMap; + import java.util.function.Supplier; /** @@ -131,19 +137,19 @@ public CommandSwerveDrivetrain( // configureAutoBuilder(); } - // public void initDriveSysid() { - // for (ModuleMap.MODULE_POSITION i : ModuleMap.MODULE_POSITION.values()) { - // var driveMotor = getModule(i.ordinal()).getDriveMotor(); - // var turnMotor = getModule(i.ordinal()).getSteerMotor(); - // TunerConstants.configureTalonFx(driveMotor, new TalonFXConfiguration()); - // TunerConstants.configureTalonFx(turnMotor, TunerConstants.generateTurnMotorConfig()); - // driveMotor.setNeutralMode(NeutralModeValue.Brake); - // BaseStatusSignal.setUpdateFrequencyForAll( - // 250, driveMotor.getPosition(), driveMotor.getVelocity(), driveMotor.getMotorVoltage()); + public void initDriveSysid() { + for (ModuleMap.MODULE_POSITION i : ModuleMap.MODULE_POSITION.values()) { + var driveMotor = getModule(i.ordinal()).getDriveMotor(); + var turnMotor = getModule(i.ordinal()).getSteerMotor(); + TunerConstants.configureTalonFx(driveMotor, new TalonFXConfiguration()); + TunerConstants.configureTalonFx(turnMotor, TunerConstants.generateTurnMotorConfig()); + driveMotor.setNeutralMode(NeutralModeValue.Brake); + BaseStatusSignal.setUpdateFrequencyForAll( + 250, driveMotor.getPosition(), driveMotor.getVelocity(), driveMotor.getMotorVoltage()); - // driveMotor.optimizeBusUtilization(); - // } - // } + driveMotor.optimizeBusUtilization(); + } + } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. * From fad1cc6819e4a3d36688cdd68e850d13888eabe8 Mon Sep 17 00:00:00 2001 From: Gavin P Date: Tue, 14 Jan 2025 17:40:29 -0800 Subject: [PATCH 03/12] Hard coded a drive forward --- src/main/java/frc/robot/RobotContainer.java | 18 ++++- .../subsystems/CommandSwerveDrivetrain.java | 17 +++-- .../java/frc/robot/utils/TrajectoryUtils.java | 76 ------------------- 3 files changed, 24 insertions(+), 87 deletions(-) delete mode 100644 src/main/java/frc/robot/utils/TrajectoryUtils.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 148f299..6c99f08 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,10 +8,15 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -116,8 +121,6 @@ private void initSysidChooser(){ SmartDashboard.putData( "initDriveSettings", new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true)); - SmartDashboard.putData( - "initTurnSettings",new InstantCommand(m_swerveDrive::initTurnSysid).ignoringDisable(true)); m_sysidChooser.addOption( "driveQuasistaticForward", @@ -177,7 +180,16 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new WaitCommand(0); + try { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); + } catch (Exception e) { + DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } } public void testInit() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2213010..b0bebfc 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.utils.CtreUtils; import frc.robot.utils.ModuleMap; import java.util.function.Supplier; @@ -134,15 +135,15 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } public void initDriveSysid() { for (ModuleMap.MODULE_POSITION i : ModuleMap.MODULE_POSITION.values()) { var driveMotor = getModule(i.ordinal()).getDriveMotor(); var turnMotor = getModule(i.ordinal()).getSteerMotor(); - TunerConstants.configureTalonFx(driveMotor, new TalonFXConfiguration()); - TunerConstants.configureTalonFx(turnMotor, TunerConstants.generateTurnMotorConfig()); + CtreUtils.configureTalonFx(driveMotor, new TalonFXConfiguration()); + CtreUtils.configureTalonFx(turnMotor, new TalonFXConfiguration() /* was previously CtreUtils.generateTurnMotorConfig() TODO: get that method working again*/); driveMotor.setNeutralMode(NeutralModeValue.Brake); BaseStatusSignal.setUpdateFrequencyForAll( 250, driveMotor.getPosition(), driveMotor.getVelocity(), driveMotor.getMotorVoltage()); @@ -169,7 +170,7 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } /** @@ -202,13 +203,13 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } // TODO: Re-implement - // public Command applyChassisSpeeds(Supplier chassisSpeeds) { - // return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); - // } +// public Command applyChassisSpeeds(Supplier chassisSpeeds) { +// return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); +// } /** * Second-Order Kinematics swerveDrive.getState().Pose, - swerveDrive::getChassisSpeed, - swerveDrive::setChassisSpeedControlNormal, - new HolonomicPathFollowerConfig( - new PIDConstants(Swerve.kP_X, TunerConstants.kI_X, TunerConstants.kD_X), - new PIDConstants(TunerConstants.kAutoP_Theta, TunerConstants.kAutoI_Theta, TunerConstants.kAutoD_Theta), - maxSpeed, - // 0.898744, - SWERVE.DRIVE.kDriveBaseRadius, - new ReplanningConfig(false, false, 1.0, 0.25)), - () -> manualFlip || Controls.isRedAlliance(), - swerveDrive); - } - - public static FollowPathHolonomic generateStartingPPHolonomicCommand( - CommandSwerveDrivetrain swerveDrive, - PathPlannerPath path, - double maxSpeed, - boolean manualFlip) { - return new FollowPathHolonomic( - path, - () -> swerveDrive.getState().Pose, - swerveDrive::getChassisSpeed, - swerveDrive::setChassisSpeedControlNormal, - new HolonomicPathFollowerConfig( - new PIDConstants(DRIVE.kP_X, DRIVE.kI_X, DRIVE.kD_X), - new PIDConstants(DRIVE.kAutoP_Theta, DRIVE.kAutoI_Theta, DRIVE.kAutoD_Theta), - maxSpeed, - // 0.898744, - SWERVE.DRIVE.kDriveBaseRadius, - new ReplanningConfig(false, false, 1.0, 0.25)), - () -> manualFlip || Controls.isRedAlliance(), - swerveDrive); - } -} From 128ab9a323fd41bc6f0d49ecb597791c663798fc Mon Sep 17 00:00:00 2001 From: Gavin P Date: Tue, 14 Jan 2025 20:04:36 -0800 Subject: [PATCH 04/12] Added/updated settings.json Reduced length of path --- .../pathplanner/paths/DriveForward.path | 8 ++--- src/main/deploy/pathplanner/settings.json | 32 +++++++++++++++++++ 2 files changed, 36 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/paths/DriveForward.path b/src/main/deploy/pathplanner/paths/DriveForward.path index a92d15b..212478d 100644 --- a/src/main/deploy/pathplanner/paths/DriveForward.path +++ b/src/main/deploy/pathplanner/paths/DriveForward.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 1.6, + "y": 5.9878289473684205 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 2.1227384868421053, + "y": 5.99391447368421 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..72e780f --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9398, + "robotLength": 0.9398, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 68.0, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.0508, + "driveGearing": 5.36, + "maxDriveSpeed": 5.96, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.34925, + "flModuleY": 0.34925, + "frModuleX": 0.34925, + "frModuleY": -0.34925, + "blModuleX": -0.34925, + "blModuleY": 0.34925, + "brModuleX": -0.34925, + "brModuleY": -0.34925, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file From f13971dc148eaad2a129fac200fda430c4911c0e Mon Sep 17 00:00:00 2001 From: Gavin P Date: Fri, 17 Jan 2025 19:21:22 -0800 Subject: [PATCH 05/12] Fixed starting pose issue (untested) Set up auto chooser --- src/main/java/frc/robot/RobotContainer.java | 23 ++++------ .../java/frc/robot/commands/DriveForward.java | 46 +++++++++++++++++++ 2 files changed, 56 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveForward.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c99f08..d70ae66 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,10 +22,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.DriveForward; import frc.robot.commands.ResetGyro; import frc.robot.commands.RunAlgaeIntake; import frc.robot.commands.RunCoralOuttake; import frc.robot.commands.SwerveCharacterization; +import frc.robot.constants.ROBOT; import frc.robot.constants.SWERVE; import frc.robot.constants.USB; import frc.robot.constants.SWERVE.ROUTINE_TYPE; @@ -78,7 +80,7 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - initAutoChooser(); + initSmartDashboard(); } private void initializeSubSystems() { @@ -100,11 +102,15 @@ private void initializeSubSystems() { private void initAutoChooser() { SmartDashboard.putData("Auto Mode", m_chooser); + + m_chooser.addOption("Do Nothing", new WaitCommand(0)); + + m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); } private void initSmartDashboard() { - // if (ROBOT.useSysID) initSysidChooser(); - // else initAutoChooser(); + if (ROBOT.useSysID) initSysidChooser(); + else initAutoChooser(); SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive)); } @@ -180,16 +186,7 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - try { - // Load the path you want to follow using its name in the GUI - PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); - - // Create a path following command using AutoBuilder. This will also trigger event markers. - return AutoBuilder.followPath(path); - } catch (Exception e) { - DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } + return m_chooser.getSelected(); } public void testInit() { diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java new file mode 100644 index 0000000..acff6e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveForward extends SequentialCommandGroup { + /** Creates a new DriveForward. */ + public DriveForward(CommandSwerveDrivetrain swerveDrive) { + try { + PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); + + var m_ppCommand = AutoBuilder.followPath(path); + + var point = new SwerveRequest.PointWheelsAt(); + var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand( // Reset the pose of the robot to the starting pose of the path + () -> swerveDrive.resetPose(path.getStartingHolonomicPose().isPresent() ? path.getStartingHolonomicPose().get() : null)), + new InstantCommand( + () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), + swerveDrive) + .alongWith(new WaitCommand(1)), + m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); + } catch (Exception e) { + DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); + addCommands(new WaitCommand(0)); + } + } +} \ No newline at end of file From d31a015fed8d4b07f4150b334fa64b8693f3c578 Mon Sep 17 00:00:00 2001 From: Gavin P Date: Fri, 17 Jan 2025 21:02:25 -0800 Subject: [PATCH 06/12] tested working auto bit jittery --- src/main/java/frc/robot/commands/DriveForward.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java index acff6e8..9ee7930 100644 --- a/src/main/java/frc/robot/commands/DriveForward.java +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -28,11 +28,14 @@ public DriveForward(CommandSwerveDrivetrain swerveDrive) { var point = new SwerveRequest.PointWheelsAt(); var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); + // Will throw an exception if the starting pose is not present + var starting_pose = path.getStartingHolonomicPose().orElseThrow(); + // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( new InstantCommand( // Reset the pose of the robot to the starting pose of the path - () -> swerveDrive.resetPose(path.getStartingHolonomicPose().isPresent() ? path.getStartingHolonomicPose().get() : null)), + () -> swerveDrive.resetPose(starting_pose)), new InstantCommand( () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), swerveDrive) From db078a05921f9211f84b3808cabed18cbd7f9b4b Mon Sep 17 00:00:00 2001 From: Gavin P Date: Fri, 17 Jan 2025 21:11:53 -0800 Subject: [PATCH 07/12] Renamed auto chooser --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d70ae66..25888dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -101,7 +101,7 @@ private void initializeSubSystems() { } private void initAutoChooser() { - SmartDashboard.putData("Auto Mode", m_chooser); + SmartDashboard.putData("Auto Chooser", m_chooser); m_chooser.addOption("Do Nothing", new WaitCommand(0)); From 8144a55ce59a386fe7ca8359e90bfa1adf53d075 Mon Sep 17 00:00:00 2001 From: gavinskycastle <62412298+gavinskycastle@users.noreply.github.com> Date: Sat, 18 Jan 2025 05:43:08 +0000 Subject: [PATCH 08/12] Automatically applied spotless --- src/main/java/frc/robot/RobotContainer.java | 63 ++++++++++--------- .../java/frc/robot/commands/DriveForward.java | 44 ++++++------- .../subsystems/CommandSwerveDrivetrain.java | 15 ++--- 3 files changed, 63 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25888dc..4d49545 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,15 +8,10 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -29,8 +24,8 @@ import frc.robot.commands.SwerveCharacterization; import frc.robot.constants.ROBOT; import frc.robot.constants.SWERVE; -import frc.robot.constants.USB; import frc.robot.constants.SWERVE.ROUTINE_TYPE; +import frc.robot.constants.USB; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.AlgaeIntake; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -102,9 +97,9 @@ private void initializeSubSystems() { private void initAutoChooser() { SmartDashboard.putData("Auto Chooser", m_chooser); - + m_chooser.addOption("Do Nothing", new WaitCommand(0)); - + m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); } @@ -113,48 +108,56 @@ private void initSmartDashboard() { else initAutoChooser(); SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive)); } - - private void initSysidChooser(){ + + private void initSysidChooser() { SignalLogger.setPath("/media/sda1/"); SysIdUtils.createSwerveDriveRoutines(m_swerveDrive); SysIdUtils.createSwerveTurnRoutines(m_swerveDrive); SmartDashboard.putData( - "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); + "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); SmartDashboard.putData( - "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); + "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); SmartDashboard.putData( - "initDriveSettings", + "initDriveSettings", new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true)); m_sysidChooser.addOption( - "driveQuasistaticForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + "driveQuasistaticForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC)); m_sysidChooser.addOption( - "driveQuasistaticBackwards", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + "driveQuasistaticBackwards", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC)); m_sysidChooser.addOption( - "driveDynamicForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC)); + "driveDynamicForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC)); m_sysidChooser.addOption( - "driveDynamicBackward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC)); + "driveDynamicBackward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC)); m_sysidChooser.addOption( - "turnQuasistaticForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC)); + "turnQuasistaticForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC)); m_sysidChooser.addOption( - "turnQuasistaticBackwards", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC)); + "turnQuasistaticBackwards", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC)); m_sysidChooser.addOption( - "turnDynamicForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC)); + "turnDynamicForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC)); m_sysidChooser.addOption( - "turnDynamicBackward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC)); + "turnDynamicBackward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC)); + } - } /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java index 9ee7930..103ca7d 100644 --- a/src/main/java/frc/robot/commands/DriveForward.java +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -21,29 +21,29 @@ public class DriveForward extends SequentialCommandGroup { /** Creates a new DriveForward. */ public DriveForward(CommandSwerveDrivetrain swerveDrive) { try { - PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); + PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); - var m_ppCommand = AutoBuilder.followPath(path); - - var point = new SwerveRequest.PointWheelsAt(); - var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); - - // Will throw an exception if the starting pose is not present - var starting_pose = path.getStartingHolonomicPose().orElseThrow(); - - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new InstantCommand( // Reset the pose of the robot to the starting pose of the path - () -> swerveDrive.resetPose(starting_pose)), - new InstantCommand( - () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), - swerveDrive) - .alongWith(new WaitCommand(1)), - m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); + var m_ppCommand = AutoBuilder.followPath(path); + + var point = new SwerveRequest.PointWheelsAt(); + var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); + + // Will throw an exception if the starting pose is not present + var starting_pose = path.getStartingHolonomicPose().orElseThrow(); + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand( // Reset the pose of the robot to the starting pose of the path + () -> swerveDrive.resetPose(starting_pose)), + new InstantCommand( + () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), + swerveDrive) + .alongWith(new WaitCommand(1)), + m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); } catch (Exception e) { - DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); - addCommands(new WaitCommand(0)); + DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); + addCommands(new WaitCommand(0)); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b0bebfc..9fccf41 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -28,11 +28,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.utils.CtreUtils; import frc.robot.utils.ModuleMap; - import java.util.function.Supplier; /** @@ -143,14 +141,17 @@ public void initDriveSysid() { var driveMotor = getModule(i.ordinal()).getDriveMotor(); var turnMotor = getModule(i.ordinal()).getSteerMotor(); CtreUtils.configureTalonFx(driveMotor, new TalonFXConfiguration()); - CtreUtils.configureTalonFx(turnMotor, new TalonFXConfiguration() /* was previously CtreUtils.generateTurnMotorConfig() TODO: get that method working again*/); + CtreUtils.configureTalonFx( + turnMotor, + new TalonFXConfiguration() /* was previously CtreUtils.generateTurnMotorConfig() TODO: get that method working again*/); driveMotor.setNeutralMode(NeutralModeValue.Brake); BaseStatusSignal.setUpdateFrequencyForAll( 250, driveMotor.getPosition(), driveMotor.getVelocity(), driveMotor.getMotorVoltage()); driveMotor.optimizeBusUtilization(); } - } + } + /** * Constructs a CTRE SwerveDrivetrain using the specified constants. * @@ -207,9 +208,9 @@ public CommandSwerveDrivetrain( } // TODO: Re-implement -// public Command applyChassisSpeeds(Supplier chassisSpeeds) { -// return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); -// } + // public Command applyChassisSpeeds(Supplier chassisSpeeds) { + // return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); + // } /** * Second-Order Kinematics Date: Fri, 17 Jan 2025 22:01:48 -0800 Subject: [PATCH 09/12] Code cleanup --- src/main/java/frc/robot/RobotContainer.java | 11 +++----- .../java/frc/robot/commands/DriveForward.java | 5 +--- .../frc/robot/constants/CORALOUTTAKE.java | 4 +-- src/main/java/frc/robot/constants/SWERVE.java | 2 ++ src/main/java/frc/robot/constants/USB.java | 2 +- .../java/frc/robot/subsystems/Controls.java | 1 - .../frc/robot/subsystems/CoralOuttake.java | 11 ++++++-- .../java/frc/robot/subsystems/Elevator.java | 25 ------------------- src/main/java/frc/robot/utils/CtreUtils.java | 4 +-- 9 files changed, 19 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4d49545..ae5d321 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,7 +75,6 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - initSmartDashboard(); } private void initializeSubSystems() { @@ -168,13 +167,9 @@ private void initSysidChooser() { * joysticks}. */ private void configureBindings() { - // // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - // new Trigger(m_exampleSubsystem::exampleCondition) - // .onTrue(new ExampleCommand(m_exampleSubsystem)); - // - // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // // cancelling on release. - // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // cancelling on release. + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); m_driverController.leftBumper().whileTrue(new RunCoralOuttake(m_coralOuttake, 0.15)); // outtake m_driverController .rightBumper() diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java index 103ca7d..73cf85b 100644 --- a/src/main/java/frc/robot/commands/DriveForward.java +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -14,12 +14,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.CommandSwerveDrivetrain; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DriveForward extends SequentialCommandGroup { /** Creates a new DriveForward. */ - public DriveForward(CommandSwerveDrivetrain swerveDrive) { + public DriveForward(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) { try { PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); diff --git a/src/main/java/frc/robot/constants/CORALOUTTAKE.java b/src/main/java/frc/robot/constants/CORALOUTTAKE.java index 0718b69..c00624d 100644 --- a/src/main/java/frc/robot/constants/CORALOUTTAKE.java +++ b/src/main/java/frc/robot/constants/CORALOUTTAKE.java @@ -7,6 +7,6 @@ public class CORALOUTTAKE { public static final double kI = 0.0; public static final double kD = 0.0; public static final double gearRatio = 1.0; - public static final double Inertia = 0.001; - public static final DCMotor Gearbox = DCMotor.getKrakenX60(1); + public static final double inertia = 0.001; + public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } diff --git a/src/main/java/frc/robot/constants/SWERVE.java b/src/main/java/frc/robot/constants/SWERVE.java index ceb63f9..c15f340 100644 --- a/src/main/java/frc/robot/constants/SWERVE.java +++ b/src/main/java/frc/robot/constants/SWERVE.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +// TODO: most of these values have been moved to delete or update public class SWERVE { public static double kTrackWidth = Units.inchesToMeters(21); public static final double kWheelBase = Units.inchesToMeters(19); @@ -24,6 +25,7 @@ public class SWERVE { // public static final SwerveDriveKinematics kSwerveKinematics = // new SwerveDriveKinematics( // ModuleMap.orderedValues(kModuleTranslations, new Translation2d[0])); + public static final Translation2d kFrontLeftPosition = new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0); public static final Translation2d kFrontRightPosition = diff --git a/src/main/java/frc/robot/constants/USB.java b/src/main/java/frc/robot/constants/USB.java index 1d8d7c7..3b49457 100644 --- a/src/main/java/frc/robot/constants/USB.java +++ b/src/main/java/frc/robot/constants/USB.java @@ -6,4 +6,4 @@ public final class USB { public static final int xBoxController = 2; public static final int testController = 3; -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Controls.java b/src/main/java/frc/robot/subsystems/Controls.java index 28b97f9..d60604f 100644 --- a/src/main/java/frc/robot/subsystems/Controls.java +++ b/src/main/java/frc/robot/subsystems/Controls.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Controls extends SubsystemBase { - private CommandSwerveDrivetrain m_swervedrive; private static DriverStation.Alliance m_allianceColor = DriverStation.Alliance.Red; /** Creates a new Controls. */ diff --git a/src/main/java/frc/robot/subsystems/CoralOuttake.java b/src/main/java/frc/robot/subsystems/CoralOuttake.java index 8ef4080..4b85cf2 100644 --- a/src/main/java/frc/robot/subsystems/CoralOuttake.java +++ b/src/main/java/frc/robot/subsystems/CoralOuttake.java @@ -14,6 +14,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.networktables.DoubleSubscriber; @@ -29,10 +31,10 @@ public class CoralOuttake extends SubsystemBase { private boolean m_isOuttaking = false; - private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1); + private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1); private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor); private final DCMotorSim outtakeMotorSim = - new DCMotorSim(outtakePlant, CORALOUTTAKE.Gearbox); // TODO implement sim code + new DCMotorSim(outtakePlant, CORALOUTTAKE.gearbox); // TODO implement sim code private double m_desiredPercentOutput; private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0); private double m_rpmSetpoint; @@ -144,4 +146,9 @@ private void updateSmartDashboard() { SmartDashboard.putNumber("CoralOuttake/DesiredPercentOutput", m_desiredPercentOutput); SmartDashboard.putNumber("CoralOuttake/rpmSetpoint", m_rpmSetpoint); } + + @Override + public void periodic() { + updateSmartDashboard(); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 79aa58d..0fa1942 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.utils.CtreUtils; @@ -28,30 +27,6 @@ public Elevator() { CtreUtils.configureTalonFx(elevatormotors[1], configElevator); } - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/utils/CtreUtils.java b/src/main/java/frc/robot/utils/CtreUtils.java index f90491c..6b7316b 100644 --- a/src/main/java/frc/robot/utils/CtreUtils.java +++ b/src/main/java/frc/robot/utils/CtreUtils.java @@ -5,15 +5,13 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.*; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.CAN; -// import frc.robot.constants.SWERVE; - +// TODO: This class is also a mess public final class CtreUtils { /** * Initialize Phoenix Server by creating a dummy device. We do this so that the CANCoders don't From 12420fcc77a6b2b5927f0c9ca6def7fe1272ea31 Mon Sep 17 00:00:00 2001 From: gavinskycastle <62412298+gavinskycastle@users.noreply.github.com> Date: Sat, 18 Jan 2025 06:02:59 +0000 Subject: [PATCH 10/12] Automatically applied spotless --- src/main/java/frc/robot/constants/SWERVE.java | 2 +- src/main/java/frc/robot/constants/USB.java | 2 +- src/main/java/frc/robot/subsystems/CoralOuttake.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/SWERVE.java b/src/main/java/frc/robot/constants/SWERVE.java index c15f340..8700ca2 100644 --- a/src/main/java/frc/robot/constants/SWERVE.java +++ b/src/main/java/frc/robot/constants/SWERVE.java @@ -25,7 +25,7 @@ public class SWERVE { // public static final SwerveDriveKinematics kSwerveKinematics = // new SwerveDriveKinematics( // ModuleMap.orderedValues(kModuleTranslations, new Translation2d[0])); - + public static final Translation2d kFrontLeftPosition = new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0); public static final Translation2d kFrontRightPosition = diff --git a/src/main/java/frc/robot/constants/USB.java b/src/main/java/frc/robot/constants/USB.java index 3b49457..1d8d7c7 100644 --- a/src/main/java/frc/robot/constants/USB.java +++ b/src/main/java/frc/robot/constants/USB.java @@ -6,4 +6,4 @@ public final class USB { public static final int xBoxController = 2; public static final int testController = 3; -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/CoralOuttake.java b/src/main/java/frc/robot/subsystems/CoralOuttake.java index 4b85cf2..1b77d78 100644 --- a/src/main/java/frc/robot/subsystems/CoralOuttake.java +++ b/src/main/java/frc/robot/subsystems/CoralOuttake.java @@ -146,7 +146,7 @@ private void updateSmartDashboard() { SmartDashboard.putNumber("CoralOuttake/DesiredPercentOutput", m_desiredPercentOutput); SmartDashboard.putNumber("CoralOuttake/rpmSetpoint", m_rpmSetpoint); } - + @Override public void periodic() { updateSmartDashboard(); From a19fe726c55208d0cfadb7f1d1954016013232b9 Mon Sep 17 00:00:00 2001 From: Ronan-B Date: Sat, 18 Jan 2025 13:30:02 -0800 Subject: [PATCH 11/12] Auto stash before checking out "origin/drive-auto-testing" --- .../pathplanner/paths/DriveForward.path | 14 +-- .../deploy/pathplanner/paths/New Path.path | 86 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 13 +-- 3 files changed, 101 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/DriveForward.path b/src/main/deploy/pathplanner/paths/DriveForward.path index 212478d..ba52bbc 100644 --- a/src/main/deploy/pathplanner/paths/DriveForward.path +++ b/src/main/deploy/pathplanner/paths/DriveForward.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 2.425937499999999, + "x": 1.6759375, "y": 6.0 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6, + "x": 4.648, "y": 5.9878289473684205 }, "prevControl": { - "x": 2.1227384868421053, - "y": 5.99391447368421 + "x": 4.398, + "y": 5.9878289473684205 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 6.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..b030c88 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.884630681818182, + "y": 6.921747159090909 + }, + "prevControl": null, + "nextControl": { + "x": 2.134630681818182, + "y": 6.921747159090909 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.029, + "y": 6.922 + }, + "prevControl": { + "x": 3.779, + "y": 6.922 + }, + "nextControl": { + "x": 4.279, + "y": 6.922 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.029, + "y": 5.645383522727273 + }, + "prevControl": { + "x": 3.779, + "y": 5.645383522727273 + }, + "nextControl": { + "x": 4.279, + "y": 5.645383522727273 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.884630681818182, + "y": 5.645383522727273 + }, + "prevControl": { + "x": 1.6346306818181817, + "y": 5.645383522727273 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ 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 ae5d321..5847745 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,11 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -95,10 +100,9 @@ private void initializeSubSystems() { } private void initAutoChooser() { - SmartDashboard.putData("Auto Chooser", m_chooser); - - m_chooser.addOption("Do Nothing", new WaitCommand(0)); - + SmartDashboard.putData("Auto Mode", m_chooser); + m_chooser.setDefaultOption("Do Nothing", new WaitCommand(0)); + m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); } @@ -177,7 +181,6 @@ private void configureBindings() { m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * From 46fcdc52a0bd68cc0106872bcaa1d3fd1feb5001 Mon Sep 17 00:00:00 2001 From: Ronan-B <114883633+Ronan-B@users.noreply.github.com> Date: Sat, 18 Jan 2025 21:31:13 +0000 Subject: [PATCH 12/12] Automatically applied spotless --- src/main/java/frc/robot/RobotContainer.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5847745..ba1826e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,11 +8,6 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.FollowPathCommand; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -102,7 +97,7 @@ private void initializeSubSystems() { private void initAutoChooser() { SmartDashboard.putData("Auto Mode", m_chooser); m_chooser.setDefaultOption("Do Nothing", new WaitCommand(0)); - + m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); } @@ -181,6 +176,7 @@ private void configureBindings() { m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake } + /** * Use this to pass the autonomous command to the main {@link Robot} class. *