chore: format fix and new path

SaintsRobotics · Feb 27, 2024 · 9f2332c · 9f2332c
1 parent b0f6234
commit 9f2332c
Show file tree

Hide file tree

Showing 2 changed files with 214 additions and 146 deletions.
diff --git a/src/main/deploy/pathplanner/paths/BlueRightToTopClose.path b/src/main/deploy/pathplanner/paths/BlueRightToTopClose.path
@@ -0,0 +1,68 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.8020771597098753,
+        "y": 6.684305590833405
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.045306757084283,
+        "y": 7.1055916113904
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5265994976434403,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 1.5265994976434403,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    }
+  ],
+  "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": {
+    "rotation": 57.994616791916435,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -47,152 +47,152 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-    // The robot's subsystems and commands are defined here
-
-    private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
-    private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
-    private final DriveSubsystem m_robotDrive = new DriveSubsystem();
-    private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-    private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
-    private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
-    private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
-    private final SendableChooser<Command> autoChooser;
-
-    /**
-     * The container for the robot. Contains subsystems, IO devices, and commands.
-     */
-    public RobotContainer() {
-        // Registering Named Commands for autonpaths
-        NamedCommands.registerCommand("shoot", new SequentialCommandGroup(
-                new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem),
-                new WaitCommand(0.5),
-                new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)));
-
-        NamedCommands.registerCommand("intake", new SequentialCommandGroup(
-                new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
-                new NoteIntakeCommand(m_intakeSubsystem),
-                new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted),
-                new WaitCommand(0.5),
-                new NoteOuttakeCommand(m_intakeSubsystem),
-                new WaitCommand(0.5)));
-
-        // All paths automatically
-
-        autoChooser = AutoBuilder.buildAutoChooser();
-        SmartDashboard.putData("Auto Chooser", autoChooser);
-
-        AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry,
-                m_robotDrive::getChassisSpeeds,
-                m_robotDrive::autonDrive,
-                new HolonomicPathFollowerConfig(
-                        new PIDConstants(5, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
-                        DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
-                        Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in
-                                                                                           // meters. Distance
-                                                                                           // from robot center to
-                                                                                           // furthest module.
-                        new ReplanningConfig(true, true)),
-                () -> false, m_robotDrive);
-
-        m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
-
-        // Configure the trigger bindings
-        configureBindings();
-
-        m_robotDrive.setDefaultCommand(
-                new RunCommand(
-                        () -> m_robotDrive.drive(
-                                MathUtil.applyDeadband(
-                                        -m_driverController.getLeftY(),
-                                        IOConstants.kControllerDeadband)
-                                        * DriveConstants.kMaxSpeedMetersPerSecond
-                                        * (1 - m_driverController
-                                                .getLeftTriggerAxis()
-                                                * IOConstants.kSlowModeScalar),
-                                // * 0.8,
-                                MathUtil.applyDeadband(
-                                        -m_driverController.getLeftX(),
-                                        IOConstants.kControllerDeadband)
-                                        * DriveConstants.kMaxSpeedMetersPerSecond
-                                        * (1 - m_driverController
-                                                .getLeftTriggerAxis()
-                                                * IOConstants.kSlowModeScalar),
-                                // * 0.8,
-                                MathUtil.applyDeadband(
-                                        -m_driverController.getRightX(),
-                                        IOConstants.kControllerDeadband)
-                                        * DriveConstants.kMaxAngularSpeedRadiansPerSecond
-                                        * (1 - m_driverController
-                                                .getLeftTriggerAxis()
-                                                * IOConstants.kSlowModeScalar)
-                                        / 2,
-                                !m_driverController.getRightBumper()),
-                        m_robotDrive));
+  // The robot's subsystems and commands are defined here
+
+  private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
+  private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
+  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
+  private final SendableChooser<Command> autoChooser;
+
+  /**
+   * The container for the robot. Contains subsystems, IO devices, and commands.
+   */
+  public RobotContainer() {
+    // Registering Named Commands for autonpaths
+    NamedCommands.registerCommand("shoot", new SequentialCommandGroup(
+        new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem),
+        new WaitCommand(0.5),
+        new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)));
+
+    NamedCommands.registerCommand("intake", new SequentialCommandGroup(
+        new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
+        new NoteIntakeCommand(m_intakeSubsystem),
+        new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted),
+        new WaitCommand(0.5),
+        new NoteOuttakeCommand(m_intakeSubsystem),
+        new WaitCommand(0.5)));
+
+    // All paths automatically
+
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry,
+        m_robotDrive::getChassisSpeeds,
+        m_robotDrive::autonDrive,
+        new HolonomicPathFollowerConfig(
+            new PIDConstants(5, 0.0, 0.0), // Translation PID constants
+            new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
+            DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
+            Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in
+                                                                               // meters. Distance
+                                                                               // from robot center to
+                                                                               // furthest module.
+            new ReplanningConfig(true, true)),
+        () -> false, m_robotDrive);
+
+    m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
+
+    // Configure the trigger bindings
+    configureBindings();
+
+    m_robotDrive.setDefaultCommand(
+        new RunCommand(
+            () -> m_robotDrive.drive(
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftY(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar),
+                // * 0.8,
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar),
+                // * 0.8,
+                MathUtil.applyDeadband(
+                    -m_driverController.getRightX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxAngularSpeedRadiansPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    / 2,
+                !m_driverController.getRightBumper()),
+            m_robotDrive));
+  }
+
+  /**
+   * Use this method to define your button->command mappings.
+   */
+  private void configureBindings() {
+    new JoystickButton(m_driverController, Button.kStart.value)
+        .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive));
+
+    // new JoystickButton(m_driverController, Button.kA.value).whileTrue(
+    // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new
+    // PathConstraints(
+    // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5,
+    // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
+
+    new JoystickButton(m_operatorController, Button.kX.value)
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
+    new JoystickButton(m_operatorController, Button.kY.value)
+        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
+        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
+
+    new JoystickButton(m_operatorController, Button.kA.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive));
+    new JoystickButton(m_operatorController, Button.kB.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
+
+    new Trigger(() -> {
+      return m_driverController.getLeftTriggerAxis() > 0.5;
+    }).whileTrue(
+        new SequentialCommandGroup(
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
+            new NoteIntakeCommand(m_intakeSubsystem),
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)))
+        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
+
+    new Trigger(() -> {
+      return m_driverController.getRightTriggerAxis() > 0.5;
+    }).whileTrue(
+        new SequentialCommandGroup(
+            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp),
+            new NoteOuttakeCommand(m_intakeSubsystem)))
+        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
+  }
+
+  // adding auton Path options
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    Command follow = autoChooser.getSelected();
+    PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName());
+
+    var alliance = DriverStation.getAlliance();
+    PathPlannerPath autonPath = path;
+    if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
+      autonPath = autonPath.flipPath();
     }
+    m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());
 
-    /**
-     * Use this method to define your button->command mappings.
-     */
-    private void configureBindings() {
-        new JoystickButton(m_driverController, Button.kStart.value)
-                .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive));
-
-        // new JoystickButton(m_driverController, Button.kA.value).whileTrue(
-        // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new
-        // PathConstraints(
-        // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5,
-        // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
-
-        new JoystickButton(m_operatorController, Button.kX.value)
-                .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
-                .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-        new JoystickButton(m_operatorController, Button.kY.value)
-                .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
-                .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-
-        new JoystickButton(m_operatorController, Button.kA.value)
-                .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive));
-        new JoystickButton(m_operatorController, Button.kB.value)
-                .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
-
-        new Trigger(() -> {
-            return m_driverController.getLeftTriggerAxis() > 0.5;
-        }).whileTrue(
-                new SequentialCommandGroup(
-                        new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
-                        new NoteIntakeCommand(m_intakeSubsystem),
-                        new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)))
-                .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
-
-        new Trigger(() -> {
-            return m_driverController.getRightTriggerAxis() > 0.5;
-        }).whileTrue(
-                new SequentialCommandGroup(
-                        new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp),
-                        new NoteOuttakeCommand(m_intakeSubsystem)))
-                .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
-    }
-
-    // adding auton Path options
-
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-        Command follow = autoChooser.getSelected();
-        PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName());
-
-        var alliance = DriverStation.getAlliance();
-        PathPlannerPath autonPath = path;
-        if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
-            autonPath = autonPath.flipPath();
-        }
-        m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());
-
-        // return AutoBuilder.followPath(autonPath);
-        return null;
-    }
+    // return AutoBuilder.followPath(autonPath);
+    return null;
+  }
 }