diff --git a/.Glass/glass.json b/.Glass/glass.json index e4ddda6..4b3bb52 100644 --- a/.Glass/glass.json +++ b/.Glass/glass.json @@ -22,6 +22,17 @@ "Launcher": { "Amp": { "open": true + } + }, + "Swerve": { + "Modules": { + "Back Left": { + "open": true + }, + "Back Right": { + "open": true + }, + "open": true }, "open": true }, @@ -30,7 +41,15 @@ }, "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Autonomous": "String Chooser" + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + } } }, "NetworkTables Info": { @@ -83,5 +102,6 @@ } ] } - } + }, + "enterKey": 92 } diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..f47b0a5 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": false, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 69828b8..90939e1 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.diffplug.spotless" version "6.22.0" } diff --git a/src/main/deploy/pathplanner/autos/One Piece.auto b/src/main/deploy/pathplanner/autos/One Piece.auto new file mode 100644 index 0000000..6588a93 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/One Piece.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1 Note Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "1 Note Part 2" + } + }, + { + "type": "named", + "data": { + "name": "LauncherLaunch" + } + } + ] + } + }, + "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/1 Note Part 1.path b/src/main/deploy/pathplanner/paths/1 Note Part 1.path new file mode 100644 index 0000000..88b89c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1 Note Part 1.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8574827844100136, + "y": 4.028517436664504 + }, + "prevControl": null, + "nextControl": { + "x": 0.8617584596280307, + "y": 4.017650095485377 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8779768624352067, + "y": 4.781214428169598 + }, + "prevControl": { + "x": 2.0779768624352064, + "y": 4.781214428169598 + }, + "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/1 Note Part 2.path b/src/main/deploy/pathplanner/paths/1 Note Part 2.path new file mode 100644 index 0000000..453835f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1 Note Part 2.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.88, + "y": 4.78 + }, + "prevControl": null, + "nextControl": { + "x": 2.235378507794053, + "y": 4.985437304207942 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3921797241742635, + "y": 5.5760149436965545 + }, + "prevControl": { + "x": 2.4431478839872605, + "y": 5.5760149436965545 + }, + "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": -50.079173387765984, + "rotateFast": false + }, + "reversed": true, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b21933e..34e9d50 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,9 +5,12 @@ package com.stuypulse.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive; import com.stuypulse.robot.commands.launcher.LaunchPrepare; +import com.stuypulse.robot.commands.launcher.LaunchPrepare; import com.stuypulse.robot.commands.launcher.LauncherHoldSpeed; import com.stuypulse.robot.commands.launcher.LauncherLaunchSpeaker; import com.stuypulse.robot.commands.launcher.LauncherIntakeNote; @@ -44,6 +47,7 @@ public class RobotContainer { public RobotContainer() { configureDefaultCommands(); configureButtonBindings(); + configureNamedCommands(); configureAutons(); } @@ -56,6 +60,14 @@ private void configureDefaultCommands() { launcher.setDefaultCommand(new LauncherHoldSpeed(Settings.Launcher.LAUNCHER_SPEAKER_SPEED)); } + /**********************/ + /*** NAMED COMMANDS ***/ + /**********************/ + + private void configureNamedCommands() { + NamedCommands.registerCommand("LauncherLaunch", new LauncherLaunchSpeaker()); + } + /***************/ /*** BUTTONS ***/ /***************/ @@ -70,7 +82,7 @@ private void configureDriverBindings() { .whileTrue(new LauncherIntakeNote()); driver.getBottomButton() - .whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_AMP_SPEED, Settings.Launcher.AMP_THRESHOLD_RPM).andThen(new LauncherLaunch(Settings.Launcher.FEEDER_AMP_SPEED, Settings.Launcher.LAUNCHER_AMP_SPEED))); + .whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_SPEAKER_SPEED, Settings.Launcher.SPEAKER_THRESHOLD_RPM).andThen(new LauncherLaunchSpeaker())); } private void configureOperatorBindings() {} @@ -80,7 +92,7 @@ private void configureOperatorBindings() {} /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + autonChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d6db63f..24e682f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -49,10 +49,10 @@ public interface Launcher { double LAUNCHER_INTAKE_SPEED = -1; double FEEDER_INTAKE_SPEED = -0.2; - SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 4000); + SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 2250); - SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.7); - SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.7); + SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.4); + SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.4); } public interface Alignment { diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java index d3774d8..9cc0cd8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java @@ -2,6 +2,7 @@ import com.stuypulse.stuylib.math.Angle; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java index eaddf4e..4c5f823 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java @@ -7,13 +7,27 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.math.Angle; - +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Drivetrain extends AbstractDrivetrain { + private final DifferentialDrive drivetrain; + private final DifferentialDriveKinematics kinematics; + private final DifferentialDriveOdometry odometry; + private final CANSparkMax leftFront; private final CANSparkMax leftBack; private final CANSparkMax rightFront; @@ -36,6 +50,25 @@ public Drivetrain() { Motors.Drivetrain.RIGHT.configure(rightBack); drivetrain = new DifferentialDrive(leftFront, rightFront); + + kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); + odometry = new DifferentialDriveOdometry(new Rotation2d(), getLeftDistance(), getRightDistance()); + + AutoBuilder.configureRamsete( + this::getPose, + this::resetPose, + this::getCurrentSpeeds, + this::drive, + new ReplanningConfig(), + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + ); } //********** GETTERS **********// @@ -74,8 +107,28 @@ public double getDistance() { public Angle getAngle() { return Angle.fromDegrees(Math.toDegrees(getLeftDistance() - getRightDistance() / Settings.Drivetrain.TRACK_WIDTH)); } - - //********** Drive Methods **********// + + private DifferentialDriveWheelPositions getWheelPositions() { + return new DifferentialDriveWheelPositions(getLeftDistance(), getRightDistance()); + } + + public Pose2d getPose() { + return odometry.getPoseMeters(); + } + + public void resetPose(Pose2d pose) { + odometry.resetPosition(new Rotation2d(), getWheelPositions(), pose); + } + + private double getAngularVelocity() { + return (getLeftSpeed() - getRightSpeed()) / Units.inchesToMeters(26); + } + + public ChassisSpeeds getCurrentSpeeds() { + return new ChassisSpeeds(getVelocity(), 0, getAngularVelocity()); + } + + //********** Drive Methods **********// public void tankDriveVolts(double leftVolts, double rightVolts) { leftFront.setVoltage(leftVolts); rightFront.setVoltage(rightVolts); @@ -94,6 +147,10 @@ public void curvatureDrive(double speed, double rotation, boolean isQuickTurn) { drivetrain.curvatureDrive(speed, rotation, isQuickTurn); } + public void drive(ChassisSpeeds speeds) { + drivetrain.arcadeDrive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + } + public CANSparkMax[] getMotors() { return new CANSparkMax[] { rightFront, leftFront, rightBack, leftBack @@ -122,6 +179,8 @@ public void stop() { @Override public void periodicChild() { + odometry.update(new Rotation2d(), getWheelPositions()); + SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed()); SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());