Merge branch 'main' of https://github.com/StuyPulse/2024-Kitbot

StuyPulse · Feb 2, 2024 · 43405bb · 43405bb
2 parents 9e5bb0b + 8164a75
commit 43405bb
Show file tree

Hide file tree

Showing 11 changed files with 245 additions and 11 deletions.
diff --git 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
@@ -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
+}
diff --git 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
@@ -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
+}
diff --git a/src/main/deploy/pathplanner/navgrid.json 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]]}
diff --git a/src/main/deploy/pathplanner/paths/1 Note Part 1.path 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
+}
diff --git a/src/main/deploy/pathplanner/paths/1 Note Part 2.path 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
+}
diff --git 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
@@ -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
@@ -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
@@ -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());