diff --git a/src/main/deploy/pathplanner/autos/Center_CloseBottom_CloseMid_CloseTop_FarTop.auto b/src/main/deploy/pathplanner/autos/Center_CloseBottom_CloseMid_CloseTop_FarTop.auto index 0690277..6861943 100644 --- a/src/main/deploy/pathplanner/autos/Center_CloseBottom_CloseMid_CloseTop_FarTop.auto +++ b/src/main/deploy/pathplanner/autos/Center_CloseBottom_CloseMid_CloseTop_FarTop.auto @@ -92,6 +92,12 @@ "name": "auto shoot" } }, + { + "type": "path", + "data": { + "pathName": "CloseTop_FarTop" + } + }, { "type": "parallel", "data": { @@ -99,31 +105,18 @@ { "type": "named", "data": { - "name": "intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CloseTop_FarTop2" - } - }, - { - "type": "path", - "data": { - "pathName": "FarTop_LeftStage" - } - } - ] + "name": "auto intake" } } ] } }, + { + "type": "path", + "data": { + "pathName": "FarTop_LeftStage" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path b/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path index a30c296..84fbd43 100644 --- a/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path +++ b/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path @@ -38,7 +38,7 @@ "maxAngularAcceleration": 2160.0 }, "goalEndState": { - "velocity": 0, + "velocity": 4.0, "rotation": 8.652541791114864, "rotateFast": true }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7e54d73..d698fc9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -116,7 +116,7 @@ public static class AutoNames { } public static class Drive { - public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.0), Units.Value.of(0.9)); + public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(1.1), Units.Value.of(1.0)); public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(8.0, 0.0, 0.3, 0.0, 0.0); public static final Measure DRIVE_SLIP_RATIO = Units.Percent.of(8.0); public static final double DRIVE_TURN_SCALAR = 90.0; @@ -168,40 +168,40 @@ public static class Shooter { ); public static final TrapezoidProfile.Constraints ANGLE_MOTION_CONSTRAINT = new TrapezoidProfile.Constraints( Units.DegreesPerSecond.of(360.0), - Units.DegreesPerSecond.of(360.0 * 6).per(Units.Second) + Units.DegreesPerSecond.of(360.0 * 3).per(Units.Second) ); public static final List,State>> SHOOTER_MAP = Arrays.asList( Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))), Map.entry(Units.Meters.of(1.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))), Map.entry(Units.Meters.of(1.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(55.0))), Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))), - Map.entry(Units.Meters.of(1.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(51.0))), - Map.entry(Units.Meters.of(1.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(49.0))), - Map.entry(Units.Meters.of(1.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(47.0))), - Map.entry(Units.Meters.of(1.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(46.0))), - Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(45.0))), - Map.entry(Units.Meters.of(2.10), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))), - Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(43.0))), - Map.entry(Units.Meters.of(2.30), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))), - Map.entry(Units.Meters.of(2.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(41.0))), - Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(40.0))), - Map.entry(Units.Meters.of(2.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(39.0))), - Map.entry(Units.Meters.of(2.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))), - Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))), - Map.entry(Units.Meters.of(2.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.0))), - Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.0))), - Map.entry(Units.Meters.of(3.10), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(35.0))), - Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(34.0))), - Map.entry(Units.Meters.of(3.30), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(33.0))), - Map.entry(Units.Meters.of(3.40), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))), - Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))), - Map.entry(Units.Meters.of(3.60), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(31.0))), - Map.entry(Units.Meters.of(3.70), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(30.0))), - Map.entry(Units.Meters.of(3.80), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))), - Map.entry(Units.Meters.of(3.90), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))), - Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0))), - Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(28.0))), - Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(25.0))) + Map.entry(Units.Meters.of(1.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(52.0))), + Map.entry(Units.Meters.of(1.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(50.0))), + Map.entry(Units.Meters.of(1.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(48.0))), + Map.entry(Units.Meters.of(1.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(47.0))), + Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(46.0))), + Map.entry(Units.Meters.of(2.10), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(45.0))), + Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))), + Map.entry(Units.Meters.of(2.30), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(43.0))), + Map.entry(Units.Meters.of(2.40), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))), + Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(41.0))), + Map.entry(Units.Meters.of(2.60), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(40.0))), + Map.entry(Units.Meters.of(2.70), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(39.0))), + Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))), + Map.entry(Units.Meters.of(2.90), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))), + Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))), + Map.entry(Units.Meters.of(3.10), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(36.0))), + Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(35.0))), + Map.entry(Units.Meters.of(3.30), new State(Units.MetersPerSecond.of(16.0), Units.Degrees.of(34.0))), + Map.entry(Units.Meters.of(3.40), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(34.0))), + Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(33.0))), + Map.entry(Units.Meters.of(3.60), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))), + Map.entry(Units.Meters.of(3.70), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(31.0))), + Map.entry(Units.Meters.of(3.80), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))), + Map.entry(Units.Meters.of(3.90), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))), + Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(31.0))), + Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(29.0))), + Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.0))) ); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9610322..c052e28 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ public class RobotContainer { IntakeSubsystem.initializeHardware(), Constants.Intake.ROLLER_VELOCITY ); - //private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance(); + private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance(); private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT); @@ -83,6 +83,8 @@ public RobotContainer() { // Configure auto builder DRIVE_SUBSYSTEM.configureAutoBuilder(); + VISION_SUBSYSTEM.setPoseSupplier(() -> DRIVE_SUBSYSTEM.getPose()); + // Register named commands NamedCommands.registerCommand(Constants.NamedCommands.INTAKE_COMMAND_NAME, autoIntakeCommand().withTimeout(7)); NamedCommands.registerCommand(Constants.NamedCommands.PRELOAD_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(1.1)); @@ -173,6 +175,8 @@ private void configureBindings() { () -> PRIMARY_CONTROLLER.getLeftX(), () -> PRIMARY_CONTROLLER.getRightX() )); + + PRIMARY_CONTROLLER.povLeft().whileTrue(aimAndIntakeObjectCommand()); } /** @@ -301,7 +305,7 @@ private Command feedThroughCommand() { * @return Command to aim robot at object, drive, and intake a game object */ private Command aimAndIntakeObjectCommand() { - return Commands.sequence( + // return Commands.sequence( // DRIVE_SUBSYSTEM.driveCommand(() -> 0, () -> 0, () -> 0).withTimeout(0.1), // DRIVE_SUBSYSTEM.aimAtPointCommand( // () -> VISION_SUBSYSTEM.getObjectLocation().isPresent() ? PRIMARY_CONTROLLER.getLeftY() : 0, @@ -312,22 +316,21 @@ private Command aimAndIntakeObjectCommand() { // }, // false, // false).until(() -> VISION_SUBSYSTEM.shouldIntake()), - // Commands.parallel( - // DRIVE_SUBSYSTEM.aimAtPointCommand( - // () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75, - // () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75, - // () -> 0, - // () -> { - // return VISION_SUBSYSTEM.getObjectLocation().orElse(null); - // }, - // false, - // false) - - // INTAKE_SUBSYSTEM.intakeCommand(), - // SHOOTER_SUBSYSTEM.intakeCommand() - // ) - // .until(() -> SHOOTER_SUBSYSTEM.isObjectPresent()) - ); + return Commands.parallel( + DRIVE_SUBSYSTEM.aimAtPointCommand( + () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75, + () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75, + () -> 0, + () -> { + return VISION_SUBSYSTEM.getObjectLocation().orElse(null); + }, + false, + false), + + INTAKE_SUBSYSTEM.intakeCommand(), + SHOOTER_SUBSYSTEM.intakeCommand() + ) + .until(() -> SHOOTER_SUBSYSTEM.isObjectPresent()); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index a51bd04..19ec1e5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -228,7 +228,7 @@ public ShooterSubsystem(Hardware shooterHardware, SparkPIDConfig flywheelConfig, // Set default command to track speaker angle setDefaultCommand(run(() -> { var state = getAutomaticState(); - state = new State(SPINUP_SPEED, state.angle); + state = new State(ZERO_FLYWHEEL_SPEED, state.angle); setState(state, true); })); @@ -325,7 +325,7 @@ private State normalizeState(State state) { * Reset shooter state */ private void resetState() { - setState(new State(SPINUP_SPEED, m_desiredShooterState.angle), false); + setState(new State(ZERO_FLYWHEEL_SPEED, m_desiredShooterState.angle), false); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java b/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java index ac31164..1a811cf 100644 --- a/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java @@ -21,7 +21,7 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import frc.robot.Constants; -import frc.robot.subsystems.vision.AprilTagCamera.Resolution; +import org.lasarobotics.vision.AprilTagCamera.Resolution; /** * Camera that looks for rings on the ground diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index befbede..4cf3c0c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.subsystems.vision.AprilTagCamera.AprilTagCameraResult; public class VisionSubsystem extends SubsystemBase implements AutoCloseable { @@ -92,18 +93,6 @@ private VisionSubsystem(Hardware visionHardware) { // Set field layout for sim m_sim.addAprilTags(m_fieldLayout); - // Setup camera pose estimation threads - this.m_cameraNotifier = (RobotBase.isReal()) - ? new Notifier(() -> { - for (var camera : m_apriltagCameras) camera.run(); - updateEstimatedGlobalPoses(); - }) - : new Notifier(() -> { - if (m_poseSupplier != null) m_sim.update(m_poseSupplier.get()); - for (var camera : m_apriltagCameras) camera.run(); - updateEstimatedGlobalPoses(); - }); - // Set all cameras to primary pipeline for (var camera : m_apriltagCameras) camera.setPipelineIndex(0); @@ -115,10 +104,6 @@ private VisionSubsystem(Hardware visionHardware) { // Add AprilTag cameras to sim for (var camera : m_apriltagCameras) m_sim.addCamera(camera.getCameraSim(), camera.getTransform()); - - // Start camera thread - m_cameraNotifier.setName(getName()); - m_cameraNotifier.startPeriodic(GlobalConstants.ROBOT_LOOP_PERIOD); } /** @@ -127,12 +112,12 @@ private VisionSubsystem(Hardware visionHardware) { */ private static Hardware initializeHardware() { Hardware visionHardware = new Hardware( - // new ObjectCamera( - // Constants.VisionHardware.CAMERA_OBJECT_NAME, - // Constants.VisionHardware.CAMERA_OBJECT_LOCATION, - // Constants.VisionHardware.CAMERA_OBJECT_RESOLUTION, - // Constants.VisionHardware.CAMERA_OBJECT_FOV - // ), + new ObjectCamera( + Constants.VisionHardware.CAMERA_OBJECT_NAME, + Constants.VisionHardware.CAMERA_OBJECT_LOCATION, + Constants.VisionHardware.CAMERA_OBJECT_RESOLUTION, + Constants.VisionHardware.CAMERA_OBJECT_FOV + ) // new AprilTagCamera( // Constants.VisionHardware.CAMERA_A_NAME, // Constants.VisionHardware.CAMERA_A_LOCATION,