Merge branch 'main' of https://github.com/team334/R2024

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -1,25 +1,19 @@
 {
   "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 1.9099007567375976,
-      "y": 6.998240302783657
-    },
-    "rotation": 0
-  },
+  "startingPose": null,
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
         {
           "type": "path",
           "data": {
-            "pathName": "Test Path"
+            "pathName": "New New New New Path"
           }
         }
       ]
     }
   },
   "folder": null,
   "choreoAuto": false
-}
+}

src/main/deploy/pathplanner/autos/New New Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New New New New Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/New New New New Path.path

@@ -0,0 +1,84 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.294070804357825
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0023477884793603,
+        "y": 7.294070804357825
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 1.7564936030772962
+      },
+      "prevControl": {
+        "x": 2.002347788479361,
+        "y": 1.7657383062513876
+      },
+      "nextControl": {
+        "x": 1.9976522115206392,
+        "y": 1.747248899903205
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 1.7564936030772962
+      },
+      "prevControl": {
+        "x": 2.002274563107685,
+        "y": 1.7380041967291142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.2,
+      "rotationDegrees": 0.0,
+      "rotateFast": true
+    },
+    {
+      "waypointRelativePos": 0.4,
+      "rotationDegrees": -90.0,
+      "rotateFast": true
+    },
+    {
+      "waypointRelativePos": 0.65,
+      "rotationDegrees": 180.0,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.67,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 91.31609348391021,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 90.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Really Short Path pt.1.path

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.9099007567384503,
+        "x": 1.91,
         "y": 7.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.2637184360915796,
+        "x": 2.2638176793531297,
         "y": 7.0
       },
       "isLocked": false,
@@ -58,4 +58,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
+}

src/main/deploy/pathplanner/paths/amp path.path

@@ -58,4 +58,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
+}

src/main/java/frc/robot/Constants.java

@@ -72,7 +72,7 @@ public static class Physical {
     public static final double SHOOTER_FLYWHEEL_CIRCUMFERENCE =
         2 * Math.PI * SHOOTER_FLYWHEEL_RADIUS;
 
-    public static final double ELEVATOR_GEAR_RATIO = 27; //TODO: THEY CHANGED THE GEARBOX SO THE GEAR RATIO IS DIFFERENT!!
+    public static final double ELEVATOR_GEAR_RATIO = 100; //TODO: fixed but they might change it
 
     public static final double SHOOTER_HEIGHT_STOWED = 0; // TODO: Get this value
 
@@ -106,7 +106,7 @@ public static class PID {
     public static final double BACK_LEFT_ROTATE_KP = 0.009;
 
     public static final PIDConstants PP_TRANSLATION = new PIDConstants(3.69, 0, 0);
-    public static final PIDConstants PP_ROTATION = new PIDConstants(1.21893, 0, 0);
+    public static final PIDConstants PP_ROTATION = new PIDConstants(1.21993, 0, 0);
 
     public static final double SHOOTER_FLYWHEEL_KP = 0;
 

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -15,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import frc.robot.commands.elevator.HoldElevator;
+import frc.robot.commands.shooter.AutoAim;
 import frc.robot.commands.shooter.HoldShooter;
 import frc.robot.commands.shooter.SpinShooter;
 import frc.robot.commands.swerve.BrakeSwerve;
@@ -25,6 +27,7 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
+import frc.robot.utils.UtilFuncs;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -35,7 +38,7 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final VisionSubsystem _visionSubsystem = new VisionSubsystem();
-  private final SwerveDriveSubsystem _swerveDrive = new SwerveDriveSubsystem(_visionSubsystem);
+  private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
   private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem();
   private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem();
 
@@ -54,18 +57,27 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    Command interruptSwerve = new BrakeSwerve(_swerveDrive, 3);
+    Command interruptSwerve = new BrakeSwerve(_swerveSubsystem, 3);
 
     NamedCommands.registerCommand("printHello", new PrintCommand("AUTON HELLO"));
     NamedCommands.registerCommand("waitCommand", new WaitCommand(3));
     NamedCommands.registerCommand("interruptSwerve", interruptSwerve);
 
-    _swerveDrive.setDefaultCommand(
-        new TeleopDrive(
-            _swerveDrive,
-            () -> -_driveFilterLeftY.calculate(_driveController.getLeftY()),
-            () -> -_driveFilterLeftX.calculate(_driveController.getLeftX()),
-            () -> -_driveFilterRightX.calculate(_driveController.getRightX())));
+    _swerveSubsystem.setDefaultCommand(
+      // new TeleopDrive(
+      //   _swerveDrive,
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1),
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1),
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.1)
+      // )
+      new AutoAim(
+        _shooterSubsystem,
+        _visionSubsystem,
+        _swerveSubsystem,
+        () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1),
+        () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1)
+      )
+    );
 
     // _elevatorSubsystem.setDefaultCommand(new HoldElevator(_elevatorSubsystem));
     // _shooterSubsystem.setDefaultCommand(new HoldShooter(_shooterSubsystem));
@@ -80,37 +92,37 @@ public RobotContainer() {
 
   // to configure button bindings
   private void configureBindings() {
-    _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveDrive));
-    _driveController.square().onTrue(new ResetPose(_swerveDrive));
+    _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveSubsystem));
+    _driveController.square().onTrue(new ResetPose(_swerveSubsystem));
     _driveController.circle().whileTrue(new SpinShooter(_shooterSubsystem));
-    _driveController.cross().whileTrue(new BrakeSwerve(_swerveDrive));
+    _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem));
 
     // for testing raw percent output, is it straight?
     _driveController
         .L1()
         .onTrue(
             Commands.runOnce(
                 () -> {
-                  _swerveDrive.driveTest(0.1);
+                  _swerveSubsystem.driveTest(0.1);
                 },
-                _swerveDrive));
+                _swerveSubsystem));
 
     // for testing velocity output (forward at 0.3 m/s), is it straight?
     _driveController
         .triangle()
         .whileTrue(
             Commands.run(
                 () -> {
-                  _swerveDrive.driveChassis(new ChassisSpeeds(0.3, 0, 0));
+                  _swerveSubsystem.driveChassis(new ChassisSpeeds(0.3, 0, 0));
                 },
-                _swerveDrive));
+                _swerveSubsystem));
   }
 
   /**
    * @return The Command to schedule for auton.
    */
   public Command getAutonCommand() {
-    _swerveDrive.fieldOriented =
+    _swerveSubsystem.fieldOriented =
         false; // make sure swerve is robot-relative for pathplanner to work
 
     return _autonChooser.getSelected();