From 6a08b0a2de6fed2ef77feb4b1d8e08684b35fbcd Mon Sep 17 00:00:00 2001 From: Shaun Date: Fri, 26 Jan 2024 16:28:38 -0500 Subject: [PATCH 1/3] Add Initial Pathplanner Things Added stuff to constants and Drivetrain subsystem. Not done! --- .../pathplanner/autos/Straight Line Turn.auto | 19 ++++ src/main/deploy/pathplanner/navgrid.json | 1 + src/main/deploy/pathplanner/paths/_3m.path | 49 ++++++++ .../deploy/pathplanner/paths/_3mTurn.path | 49 ++++++++ .../frc/robot/CommandSwerveDrivetrain.java | 56 --------- src/main/java/frc/robot/Constants.java | 14 +++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/commands/DriveCommand.java | 21 ++++ .../frc/robot/generated/TunerConstants.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 106 ++++++++++++++++++ vendordeps/PathplannerLib.json | 38 +++++++ 11 files changed, 300 insertions(+), 58 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Straight Line Turn.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/_3m.path create mode 100644 src/main/deploy/pathplanner/paths/_3mTurn.path delete mode 100644 src/main/java/frc/robot/CommandSwerveDrivetrain.java create mode 100644 src/main/java/frc/robot/commands/DriveCommand.java create mode 100644 src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java create mode 100644 vendordeps/PathplannerLib.json diff --git a/src/main/deploy/pathplanner/autos/Straight Line Turn.auto b/src/main/deploy/pathplanner/autos/Straight Line Turn.auto new file mode 100644 index 0000000..5222909 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Straight Line Turn.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "_3mTurn" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/_3m.path b/src/main/deploy/pathplanner/paths/_3m.path new file mode 100644 index 0000000..e2e776b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/_3m.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.7030628645242247, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 7.0 + }, + "prevControl": { + "x": 3.585786437626905, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/_3mTurn.path b/src/main/deploy/pathplanner/paths/_3mTurn.path new file mode 100644 index 0000000..479354f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/_3mTurn.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.7030628645242247, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 7.0 + }, + "prevControl": { + "x": 3.585786437626905, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/CommandSwerveDrivetrain.java deleted file mode 100644 index 977a033..0000000 --- a/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,56 +0,0 @@ -package frc.robot; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; - -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem - * so it can be used in command-based projects easily. - */ -public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 51a2e0f..f96b930 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,10 @@ package frc.robot; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + /** * The Constants class provides a convenient place for teams to hold robot-wide * numerical or boolean @@ -21,6 +25,16 @@ public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + public static class AutoConstants { + HolonomicPathFollowerConfig PathPlannerConfig = new HolonomicPathFollowerConfig( + new PIDConstants(0.5, 0.0, 0.0), // Translation PID constants + new PIDConstants(0.5, 0.0, 0.0), // Rotation PID constants + 4.5, // Max module speed, in m/s + 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ); + } + public static class CANIDs { // ! DO NOT use CAN IDs 1-9; they are reserved for the modules and Pigeon diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b3c704b..4bbc614 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.RatioMotorSubsystem; - +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.OrchestraSubsystem; import com.ctre.phoenix6.hardware.TalonFX; @@ -22,6 +22,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java new file mode 100644 index 0000000..a3e8b26 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -0,0 +1,21 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +/** + * _Drive5 + * + */ + +public class DriveCommand extends Command { + public DriveCommand() { + super(); + } + + @Override + + public void execute() { + // TODO Auto-generated method stub + super.execute(); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c69ae9a..6abc027 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,7 +8,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import edu.wpi.first.math.util.Units; -import frc.robot.CommandSwerveDrivetrain; +import frc.robot.subsystems.CommandSwerveDrivetrain; public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..bc88871 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,106 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants; + +/** + * Class that extends the Phoenix SwerveDrivetrain class and implements + * subsystem + * so it can be used in command-based projects easily. + */ +public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + ConfigPathPlanner(); + } + + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + ConfigPathPlanner(); + setControl(new SwerveRequest.RobotCentric()); + + } + + private void ConfigPathPlanner() { + AutoBuilder.configureHolonomic( + () -> getState().Pose, // Robot pose supplier + this::seedFieldRelative, // Method to reset odometry (will be called if your auto has a + // starting pose) + () -> this.m_kinematics.toChassisSpeeds(this.getState().ModuleStates), // ChassisSpeeds supplier. MUST + // BE ROBOT RELATIVE + (ChassisSpeeds speeds) -> setControl(new SwerveRequest.ApplyChassisSpeeds().withSpeeds(speeds)), // Method + // that + // will + // drive + // the + // robot + // given + // ROBOT + // RELATIVE + // ChassisSpeeds + Constants.AutoConstants.PathPlannerConfig, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..6ddd312 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.1.5", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.1.5" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.1.5", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From 49f7e2d80ba177c474d9af1d2f11043d0b2fc6e9 Mon Sep 17 00:00:00 2001 From: Shaun Date: Fri, 26 Jan 2024 16:56:38 -0500 Subject: [PATCH 2/3] Final untested touches Cleaned up imports; changed some auto logic in robot container; Added some much needed constants --- src/main/java/frc/robot/Constants.java | 26 ++++++++-- src/main/java/frc/robot/RobotContainer.java | 52 +++++++++++-------- .../java/frc/robot/commands/DriveCommand.java | 21 -------- .../subsystems/CommandSwerveDrivetrain.java | 8 +-- 4 files changed, 51 insertions(+), 56 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f96b930..1a2d3d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import static frc.robot.Constants.SystemConstants.PID; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -25,14 +26,29 @@ public static class OperatorConstants { public static final int kDriverControllerPort = 0; } - public static class AutoConstants { - HolonomicPathFollowerConfig PathPlannerConfig = new HolonomicPathFollowerConfig( - new PIDConstants(0.5, 0.0, 0.0), // Translation PID constants - new PIDConstants(0.5, 0.0, 0.0), // Rotation PID constants - 4.5, // Max module speed, in m/s + public static class SystemConstants { + + public static double MaxSpeed = 6; // 6 meters per second desired top speed + public static double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + + public static class PID { + public static double driveP = 0.3f; + public static double driveI = 0; + public static double driveD = 0.1; + public static double steerP = 0.3; + public static double steerI = 0; + public static double steerD = 0.1; + } + + public static HolonomicPathFollowerConfig PathPlannerConfig = new HolonomicPathFollowerConfig( + new PIDConstants(PID.driveP, PID.driveI, PID.driveD), // Translation PID constants + new PIDConstants(PID.steerP, PID.steerI, PID.steerD), // Rotation PID constants + MaxSpeed, // Max module speed, in m/s 0.4, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here + ); + } public static class CANIDs { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4bbc614..b8e5fa9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,43 +8,38 @@ import frc.robot.subsystems.RatioMotorSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.OrchestraSubsystem; - import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; - +import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; - +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import com.ctre.phoenix6.Utils; - import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; 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 frc.robot.generated.TunerConstants; +import static frc.robot.Constants.SystemConstants.MaxSpeed; public class RobotContainer { - // ? can we move the below 2 fields under System Constants? - private double MaxSpeed = 6; // 6 meters per second desired top speed - private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity - /* Setting up bindings for necessary control of the swerve drive platform */ private final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric - // driving in open loop + .withDeadband(MaxSpeed * 0.1) + .withRotationalDeadband(Constants.SystemConstants.MaxAngularRate * 0.1) // Add a 10% + // deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Computer! I want field-centric + // driving in open loop! private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); + private final Telemetry logger = new Telemetry(Constants.SystemConstants.MaxSpeed); + SendableChooser autoChooser; // #region Devices TalonFX shooterMotorMain; @@ -61,6 +56,12 @@ public class RobotContainer { public RobotContainer() { + // Set shuffleboard autos + + autoChooser = AutoBuilder.buildAutoChooser(); + + Shuffleboard.getTab("Autos").add("Auton", autoChooser); + shooterMotorMain = new TalonFX(Constants.CANIDs.rightFlywheelCAN); shooterMotorSub = new TalonFX(Constants.CANIDs.leftFlywheelCAN); @@ -80,13 +81,18 @@ public RobotContainer() { shooter.SetRatio(1); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-m_driverController.getLeftY() * MaxSpeed) // Drive forward - // with - // negative Y (forward) - .withVelocityY(-m_driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-m_driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with - // negative X (left) - )); + drivetrain + .applyRequest(() -> drive.withVelocityX(-m_driverController.getLeftY() * Constants.SystemConstants.MaxSpeed) // Drive + // forward + // with + // negative Y (forward) + .withVelocityY(-m_driverController.getLeftX() * MaxSpeed) // Drive left with + // negative X (left) + .withRotationalRate(-m_driverController.getRightX() * Constants.SystemConstants.MaxAngularRate) // Drive + // counterclockwise + // with + // negative X (left) + )); // Configure the trigger bindings if (Utils.isSimulation()) { @@ -120,7 +126,7 @@ public void DebugMethodSingle() { public Command getAutonomousCommand() { // An example command will be run in autonomous - return new WaitCommand(3603); + return autoChooser.getSelected(); } // end of Autonomous } // end of class diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java deleted file mode 100644 index a3e8b26..0000000 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; - -/** - * _Drive5 - * - */ - -public class DriveCommand extends Command { - public DriveCommand() { - super(); - } - - @Override - - public void execute() { - // TODO Auto-generated method stub - super.execute(); - } -} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index bc88871..31ed5a3 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -7,13 +7,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; - -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; @@ -68,7 +62,7 @@ private void ConfigPathPlanner() { // ROBOT // RELATIVE // ChassisSpeeds - Constants.AutoConstants.PathPlannerConfig, + Constants.SystemConstants.PathPlannerConfig, () -> { // Boolean supplier that controls when the path will be mirrored for the red // alliance From 45eef0296497ff57c1163c9806766a51ee0331c9 Mon Sep 17 00:00:00 2001 From: Shaun Date: Fri, 26 Jan 2024 17:03:58 -0500 Subject: [PATCH 3/3] Alter Constants to align with Naming Convention --- src/main/java/frc/robot/Constants.java | 39 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 38 +++++++++--------- .../subsystems/CommandSwerveDrivetrain.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- 4 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1a2d3d9..f509c6f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,7 +7,6 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import static frc.robot.Constants.SystemConstants.PID; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -23,27 +22,27 @@ */ public final class Constants { public static class OperatorConstants { - public static final int kDriverControllerPort = 0; + public static final int K_DRIVER_CONTROLLER_PORT = 0; } public static class SystemConstants { - public static double MaxSpeed = 6; // 6 meters per second desired top speed - public static double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + public static final double MAX_SPEED = 6; // 6 meters per second desired top speed + public static final double MAX_ANGULAR_RATE = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity public static class PID { - public static double driveP = 0.3f; - public static double driveI = 0; - public static double driveD = 0.1; - public static double steerP = 0.3; - public static double steerI = 0; - public static double steerD = 0.1; + public static final double DRIVE_P = 0.3f; + public static final double DRIVE_I = 0; + public static final double DRIVE_D = 0.1; + public static final double STEER_P = 0.3; + public static final double STEER_I = 0; + public static final double STEER_D = 0.1; } - public static HolonomicPathFollowerConfig PathPlannerConfig = new HolonomicPathFollowerConfig( - new PIDConstants(PID.driveP, PID.driveI, PID.driveD), // Translation PID constants - new PIDConstants(PID.steerP, PID.steerI, PID.steerD), // Rotation PID constants - MaxSpeed, // Max module speed, in m/s + public static final HolonomicPathFollowerConfig PATH_PLANNER_CONFIG = new HolonomicPathFollowerConfig( + new PIDConstants(PID.DRIVE_P, PID.DRIVE_I, PID.DRIVE_D), // Translation PID constants + new PIDConstants(PID.STEER_P, PID.STEER_I, PID.STEER_D), // Rotation PID constants + MAX_SPEED, // Max module speed, in m/s 0.4, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here @@ -54,14 +53,14 @@ public static class PID { public static class CANIDs { // ! DO NOT use CAN IDs 1-9; they are reserved for the modules and Pigeon - public static final int intakeCAN = 10; - public static final int indexCAN = 11; + public static final int INTAKE_CAN = 10; + public static final int INDEX_CAN = 11; - public static final int timeOfFlightID = 12; + public static final int TIME_OF_FLIGHT_ID = 12; - public static final int bassGuitar = 13; + public static final int BASS_GUITAR = 13; - public static final int rightFlywheelCAN = 15; - public static final int leftFlywheelCAN = 16; + public static final int RIGHT_FLYWHEEL_CAN = 15; + public static final int LEFT_FLYWHEEL_CAN = 16; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b8e5fa9..3d9007f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.generated.TunerConstants; -import static frc.robot.Constants.SystemConstants.MaxSpeed; + +import static frc.robot.Constants.SystemConstants.MAX_SPEED; public class RobotContainer { @@ -31,14 +32,14 @@ public class RobotContainer { private final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(Constants.SystemConstants.MaxAngularRate * 0.1) // Add a 10% + .withDeadband(MAX_SPEED * 0.1) + .withRotationalDeadband(Constants.SystemConstants.MAX_ANGULAR_RATE * 0.1) // Add a 10% // deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Computer! I want field-centric // driving in open loop! private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(Constants.SystemConstants.MaxSpeed); + private final Telemetry logger = new Telemetry(Constants.SystemConstants.MAX_SPEED); SendableChooser autoChooser; // #region Devices @@ -52,7 +53,7 @@ public class RobotContainer { // #endregion Subsystems private final CommandXboxController m_driverController = new CommandXboxController( - OperatorConstants.kDriverControllerPort); + OperatorConstants.K_DRIVER_CONTROLLER_PORT); public RobotContainer() { @@ -62,8 +63,8 @@ public RobotContainer() { Shuffleboard.getTab("Autos").add("Auton", autoChooser); - shooterMotorMain = new TalonFX(Constants.CANIDs.rightFlywheelCAN); - shooterMotorSub = new TalonFX(Constants.CANIDs.leftFlywheelCAN); + shooterMotorMain = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_CAN); + shooterMotorSub = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_CAN); // #region some configs @@ -72,8 +73,8 @@ public RobotContainer() { shooterMotorMain.setNeutralMode(NeutralModeValue.Coast); shooterMotorMain.setNeutralMode(NeutralModeValue.Coast); - shooterMotorSub = new TalonFX(Constants.CANIDs.leftFlywheelCAN); - shooterMotorMain = new TalonFX(Constants.CANIDs.rightFlywheelCAN); + shooterMotorSub = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_CAN); + shooterMotorMain = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_CAN); // #endregion shooter = new RatioMotorSubsystem(shooterMotorMain, shooterMotorSub); @@ -82,15 +83,16 @@ public RobotContainer() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain - .applyRequest(() -> drive.withVelocityX(-m_driverController.getLeftY() * Constants.SystemConstants.MaxSpeed) // Drive - // forward - // with - // negative Y (forward) - .withVelocityY(-m_driverController.getLeftX() * MaxSpeed) // Drive left with - // negative X (left) - .withRotationalRate(-m_driverController.getRightX() * Constants.SystemConstants.MaxAngularRate) // Drive - // counterclockwise - // with + .applyRequest( + () -> drive.withVelocityX(-m_driverController.getLeftY() * Constants.SystemConstants.MAX_SPEED) // Drive + // forward + // with + // negative Y (forward) + .withVelocityY(-m_driverController.getLeftX() * MAX_SPEED) // Drive left with + // negative X (left) + .withRotationalRate(-m_driverController.getRightX() * Constants.SystemConstants.MAX_ANGULAR_RATE) // Drive + // counterclockwise + // with // negative X (left) )); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 31ed5a3..97dd8d8 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -62,7 +62,7 @@ private void ConfigPathPlanner() { // ROBOT // RELATIVE // ChassisSpeeds - Constants.SystemConstants.PathPlannerConfig, + Constants.SystemConstants.PATH_PLANNER_CONFIG, () -> { // Boolean supplier that controls when the path will be mirrored for the red // alliance diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 35e4066..cfe2e9e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -27,7 +27,7 @@ public IntakeSubsystem() { dutyCycle = new DutyCycleOut(0); motor.setControl(dutyCycle); - sensor = new TimeOfFlight(Constants.CANIDs.timeOfFlightID); + sensor = new TimeOfFlight(Constants.CANIDs.TIME_OF_FLIGHT_ID); // Every 20ms it updates () sensor.setRangingMode(RangingMode.Short, 1); }