diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 91daa27..c453562 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,8 +4,9 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 4.3, + "defaultMaxVel": 2.3, "defaultMaxAccel": 4.3, "defaultMaxAngVel": 1080.0, - "defaultMaxAngAccel": 360.0 + "defaultMaxAngAccel": 360.0, + "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leave.path b/src/main/deploy/pathplanner/paths/Leave.path index db29e7d..0b09015 100644 --- a/src/main/deploy/pathplanner/paths/Leave.path +++ b/src/main/deploy/pathplanner/paths/Leave.path @@ -38,7 +38,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.3, + "maxVelocity": 2.3, "maxAcceleration": 4.3, "maxAngularVelocity": 1080.0, "maxAngularAcceleration": 360.0 diff --git a/src/main/deploy/pathplanner/paths/pick up first note ring.path b/src/main/deploy/pathplanner/paths/pick up first note ring.path index 66e7cb0..b0bafef 100644 --- a/src/main/deploy/pathplanner/paths/pick up first note ring.path +++ b/src/main/deploy/pathplanner/paths/pick up first note ring.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.3, + "maxVelocity": 2.3, "maxAcceleration": 4.3, "maxAngularVelocity": 1080.0, "maxAngularAcceleration": 360.0 diff --git a/src/main/deploy/pathplanner/paths/pickup second note ring.path b/src/main/deploy/pathplanner/paths/pickup second note ring.path index b50fc23..74fb219 100644 --- a/src/main/deploy/pathplanner/paths/pickup second note ring.path +++ b/src/main/deploy/pathplanner/paths/pickup second note ring.path @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.3, + "maxVelocity": 2.3, "maxAcceleration": 4.3, "maxAngularVelocity": 1080.0, "maxAngularAcceleration": 360.0 diff --git a/src/main/deploy/pathplanner/paths/pickup third note ring.path b/src/main/deploy/pathplanner/paths/pickup third note ring.path index 535d36b..e31f206 100644 --- a/src/main/deploy/pathplanner/paths/pickup third note ring.path +++ b/src/main/deploy/pathplanner/paths/pickup third note ring.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.3, + "maxVelocity": 2.3, "maxAcceleration": 4.3, "maxAngularVelocity": 1080.0, "maxAngularAcceleration": 360.0 diff --git a/src/main/deploy/pathplanner/paths/shoot preload ring.path b/src/main/deploy/pathplanner/paths/shoot preload ring.path index 5b11ffa..0edc8f0 100644 --- a/src/main/deploy/pathplanner/paths/shoot preload ring.path +++ b/src/main/deploy/pathplanner/paths/shoot preload ring.path @@ -38,7 +38,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.3, + "maxVelocity": 2.3, "maxAcceleration": 4.3, "maxAngularVelocity": 1080.0, "maxAngularAcceleration": 360.0 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 92c4940..48f5e06 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,7 +68,7 @@ public static class HID { public static class Drive { public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(7.0, 0.0, 0.1, 0.0, 0.0); - public static final double DRIVE_SLIP_RATIO = 0.11; + public static final double DRIVE_SLIP_RATIO = 0.12; public static final double DRIVE_TURN_SCALAR = 60.0; public static final double DRIVE_LOOKAHEAD = 6; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cf897c..deefe4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,10 +7,13 @@ import com.revrobotics.REVPhysicsSim; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.WaggleSubsystem; +import frc.robot.subsystems.drive.AutoTrajectory; import frc.robot.subsystems.drive.DriveSubsystem; public class RobotContainer { @@ -29,6 +32,8 @@ public class RobotContainer { private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT); + private static SendableChooser m_automodeChooser = new SendableChooser<>(); + public RobotContainer() { // Set drive command DRIVE_SUBSYSTEM.setDefaultCommand( @@ -42,6 +47,9 @@ public RobotContainer() { // Setup AutoBuilder DRIVE_SUBSYSTEM.configureAutoBuilder(); + autoModeChooser(); + SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser); + // Bind buttons and triggers configureBindings(); } @@ -74,11 +82,21 @@ public void simulationPeriodic() { REVPhysicsSim.getInstance().run(); } + /** + * Add auto modes to chooser + */ + private void autoModeChooser() { + m_automodeChooser.setDefaultOption("Do nothing", Commands.none()); + m_automodeChooser.addOption("Leave", new AutoTrajectory(DRIVE_SUBSYSTEM, "Leave").getCommand()); + m_automodeChooser.addOption("Preload + 3 Ring", new AutoTrajectory(DRIVE_SUBSYSTEM, "Preload + 3 Ring").getCommand()); + m_automodeChooser.addOption("Preload + 1", new AutoTrajectory(DRIVE_SUBSYSTEM, "Preload + 1").getCommand()); + } + /** * Get currently selected autonomous command * @return Autonomous command */ public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return m_automodeChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 9cf33ea..6999d86 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -94,8 +94,8 @@ public Hardware(NavX2 navx, public static final Measure DRIVE_WHEELBASE = Units.Meters.of(0.62); public static final Measure DRIVE_TRACK_WIDTH = Units.Meters.of(0.62); public static final Measure