From 290ca307bb72b5b656dd4472c8d5fbec4bad6d20 Mon Sep 17 00:00:00 2001 From: Simon <87281874+simonp17@users.noreply.github.com> Date: Tue, 16 Jul 2024 18:58:01 -0700 Subject: [PATCH] Start adding auto capabilities for Note Map --- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/auto_manager/AutoManager.java | 73 +++++++++++++++++++ .../robot/auto_manager/AutoNoteAction.java | 10 +++ .../frc/robot/auto_manager/AutoNoteStep.java | 51 +++++++++++++ .../note_tracking/NoteTrackingManager.java | 19 +++-- 5 files changed, 151 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/auto_manager/AutoManager.java create mode 100644 src/main/java/frc/robot/auto_manager/AutoNoteAction.java create mode 100644 src/main/java/frc/robot/auto_manager/AutoNoteStep.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b6fb1de..d29dab4b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.auto_manager.AutoManager; import frc.robot.autos.Autos; import frc.robot.climber.ClimberSubsystem; import frc.robot.config.RobotConfig; @@ -97,6 +98,7 @@ public class Robot extends TimedRobot { new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision, intake); private final NoteTrackingManager noteTrackingManager = new NoteTrackingManager(localization, swerve, actions, robotManager, imu); + private final AutoManager autoManager = new AutoManager(actions, noteTrackingManager, robotManager); public Robot() { System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER); @@ -239,7 +241,7 @@ private void configureBindings() { .onFalse(actions.stowCommand()); operatorController .povRight() - .onTrue(noteTrackingManager.intakeNearestMapNote()) + .onTrue(autoManager.testCommand()) .onFalse(actions.stowCommand()); operatorController .rightTrigger() diff --git a/src/main/java/frc/robot/auto_manager/AutoManager.java b/src/main/java/frc/robot/auto_manager/AutoManager.java new file mode 100644 index 00000000..665613bb --- /dev/null +++ b/src/main/java/frc/robot/auto_manager/AutoManager.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.auto_manager; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.intake.IntakeSubsystem; +import frc.robot.note_manager.NoteManager; +import frc.robot.note_tracking.NoteMapElement; +import frc.robot.note_tracking.NoteTrackingManager; +import frc.robot.robot_manager.RobotCommands; +import frc.robot.robot_manager.RobotManager; +import frc.robot.robot_manager.RobotState; + +import java.util.ArrayList; +import java.util.List; + +public class AutoManager { + private final RobotCommands actions; + private final NoteTrackingManager noteTrackingManager; + private final RobotManager robotManager; + + public AutoManager(RobotCommands actions, NoteTrackingManager noteTrackingManager, RobotManager robotManager) { + this.actions = actions; + this.noteTrackingManager = noteTrackingManager; + this.robotManager = robotManager; + } + + public Command doManyAutoSteps(List steps) { + return Commands.sequence(steps.stream().map(this::doAutoStep).toArray(Command[]::new)); + } + + private Command doAutoStep(AutoNoteStep step) { + var command = noteTrackingManager.intakeNoteAtPose(step.noteSearchPose().get()); + + // TODO: Pathfind to outtake/shoot position + if (step.action() == AutoNoteAction.OUTTAKE) { + command = command.andThen(actions.shooterOuttakeCommand().unless(()-> !robotManager.getState().hasNote)); + } else if (step.action() == AutoNoteAction.SCORE) { + command = command.andThen(actions.speakerShotCommand().unless(()-> !robotManager.getState().hasNote)); + } + + return command; + } + + public Command testCommand() { + + return Commands.sequence( + Commands.runOnce( + () -> { + var now = Timer.getFPGATimestamp(); + // 1. reset note map as if it were start of auto + noteTrackingManager.resetNoteMap( + new ArrayList<>( + List.of( + // new NoteMapElement(now + 5, new Pose2d(8.271, 7.458, new Rotation2d(0))), + new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(4)), + new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(5)), + new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(6))))); + }), + // 2. intakes notes 4 5 6 and scores + doManyAutoSteps(List.of( + new AutoNoteStep(4, AutoNoteAction.OUTTAKE), + new AutoNoteStep(5, AutoNoteAction.SCORE), + new AutoNoteStep(6, AutoNoteAction.SCORE) + ))); + } +} diff --git a/src/main/java/frc/robot/auto_manager/AutoNoteAction.java b/src/main/java/frc/robot/auto_manager/AutoNoteAction.java new file mode 100644 index 00000000..91f20d80 --- /dev/null +++ b/src/main/java/frc/robot/auto_manager/AutoNoteAction.java @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.auto_manager; + +public enum AutoNoteAction { + SCORE, + OUTTAKE; +} diff --git a/src/main/java/frc/robot/auto_manager/AutoNoteStep.java b/src/main/java/frc/robot/auto_manager/AutoNoteStep.java new file mode 100644 index 00000000..bc391261 --- /dev/null +++ b/src/main/java/frc/robot/auto_manager/AutoNoteStep.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.auto_manager; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.fms.FmsSubsystem; +import java.util.function.Supplier; + +public record AutoNoteStep(Supplier noteSearchPose, AutoNoteAction action) { + public static Pose2d noteIdToPose(int noteId) { + switch (noteId) { + case 1: + if (FmsSubsystem.isRedAlliance()) { + return new Pose2d(13.65, 4.106, new Rotation2d(0)); + } else { + return new Pose2d(2.896, 4.106, new Rotation2d(0)); + } + case 2: + if (FmsSubsystem.isRedAlliance()) { + return new Pose2d(13.65, 5.553, new Rotation2d(0)); + } else { + return new Pose2d(2.896, 5.553, new Rotation2d(0)); + } + case 3: + if (FmsSubsystem.isRedAlliance()) { + return new Pose2d(13.65, 7.001, new Rotation2d(0)); + } else { + return new Pose2d(2.896, 7.001, new Rotation2d(0)); + } + case 4: + return new Pose2d(8.271, 7.458, new Rotation2d(0)); + case 5: + return new Pose2d(8.271, 5.782, new Rotation2d(0)); + case 6: + return new Pose2d(8.271, 4.106, new Rotation2d(0)); + case 7: + return new Pose2d(8.271, 2.429, new Rotation2d(0)); + case 8: + return new Pose2d(8.271, 0.753, new Rotation2d(0)); + default: + throw new IllegalArgumentException("Expected a note ID from between 1 and 8"); + } + } + + public AutoNoteStep(int noteId, AutoNoteAction action) { + this(() -> noteIdToPose(noteId), action); + } +} diff --git a/src/main/java/frc/robot/note_tracking/NoteTrackingManager.java b/src/main/java/frc/robot/note_tracking/NoteTrackingManager.java index 2956959b..6cf82a0b 100644 --- a/src/main/java/frc/robot/note_tracking/NoteTrackingManager.java +++ b/src/main/java/frc/robot/note_tracking/NoteTrackingManager.java @@ -30,6 +30,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.function.Supplier; public class NoteTrackingManager extends LifecycleSubsystem { @@ -76,7 +77,7 @@ public void resetNoteMap(ArrayList startingValues) { * Get the pose of the note closest to the provided location, within a threshold. Returns * optional.empty if no notes are tracked or notes exceed the threshold. */ - public Optional getNearestNotePoseRelative( + private Optional getNearestNotePoseRelative( Pose2d searchLocation, double thresholdMeters) { var maybeElement = @@ -232,13 +233,18 @@ private void removeNote(NoteMapElement note) { noteMap.remove(note); } - public Command intakeNearestMapNote() { + public Command intakeNoteAtPose(Pose2d searchPose) { + return intakeNoteAtPose(() -> searchPose); + } + + public Command intakeNoteAtPose(Supplier searchPose) { + // TODO: If no note found, exit the command (somehow) return actions .intakeCommand() .alongWith( swerve.driveToPoseCommand( () -> { - var nearestNote = getNearestNotePoseRelative(getPose(), 5); + var nearestNote = getNearestNotePoseRelative(searchPose.get(), 1.5); if (nearestNote.isPresent()) { snaps.setAngle(nearestNote.get().notePose().getRotation()); @@ -252,11 +258,11 @@ public Command intakeNearestMapNote() { }, this::getPose, false)) - .until(() -> robot.getState() == RobotState.IDLE_WITH_GP) + .until(() -> robot.getState() == RobotState.IDLE_WITH_GP || getNearestNotePoseRelative(searchPose.get(), 1.5).isEmpty()) .andThen( Commands.runOnce( () -> { - var intakedNote = getNearestNotePoseRelative(getPose(), 0.5); + var intakedNote = getNearestNotePoseRelative(searchPose.get(), 0.5); if (intakedNote.isPresent()) { removeNote(intakedNote.get()); } @@ -264,6 +270,9 @@ public Command intakeNearestMapNote() { .withName("IntakeNearestNoteCommand"); } + public Command intakeNearestMapNote() { + return intakeNoteAtPose(this::getPose); + } private Pose2d getPose() { return localization.getPose();