From 3aa92647099a3ab1abc6507d9f75ec5933b88a7f Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Fri, 12 Apr 2024 16:12:09 -0400 Subject: [PATCH] Shoot While Move and Auton changes --- .../robot/commands/AutonomousCommands.java | 58 ++++++++++++------- .../robot/commands/ShootCommands.java | 7 ++- .../robot/simulation/NoteSimulator.java | 4 +- .../com/team1701/robot/states/RobotState.java | 4 +- 4 files changed, 46 insertions(+), 27 deletions(-) diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index 19ef210d..315d7b80 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -120,6 +120,10 @@ private Command setNoteToSeek(AutoNote note) { .withName("SetNoteToSeek"); } + private Command clearNoteToSeek() { + return runOnce(() -> mAutoNoteSeeker.clear()).ignoringDisable(true).withName("ClearNoteToSeek"); + } + private Command timedDriveWithVelocity(ChassisSpeeds speeds, double seconds) { return DriveCommands.driveWithVelocity(() -> speeds, mDrive) .withTimeout(seconds) @@ -203,26 +207,39 @@ private Command driveBackPreWarmAndShoot(String pathName) { runOnce(() -> mRobotState.setUseAutonFallback(false))); } - private Command driveToNextPiece(AutoNote nextNote) { - return new DriveAndSeekNote( - mDrive, - mRobotState, - new DriveToPose( - mDrive, - mRobotState, - () -> new Pose2d( - nextNote.pose().getTranslation(), + private class NextNote { + Pose2d notePose; + + public NextNote() { + notePose = new Pose2d(); + } + } + + private Command driveToNextPiece(AutoNote note) { + NextNote nextNote = new NextNote(); + return loggedSequence( + setNoteToSeek(note), + runOnce(() -> nextNote.notePose = new Pose2d( + note.pose().getTranslation(), mRobotState .getPose2d() .getTranslation() - .minus(nextNote.pose().getTranslation()) - .getAngle()), - mRobotState::getPose2d, - kAutoTrapezoidalKinematicLimits, - true, - true), - mAutoNoteSeeker::getDetectedNoteToSeek, - kAutoTrapezoidalKinematicLimits); + .minus(note.pose().getTranslation()) + .getAngle()) + .transformBy(Constants.Robot.kIntakeToRobot)), + new DriveAndSeekNote( + mDrive, + mRobotState, + new DriveToPose( + mDrive, + mRobotState, + () -> nextNote.notePose, + mRobotState::getPose2d, + kAutoTrapezoidalKinematicLimits, + true, + true), + mAutoNoteSeeker::getDetectedNoteToSeek, + kAutoTrapezoidalKinematicLimits)); } private Pose2d getFirstPose(String pathName) { @@ -254,7 +271,7 @@ private Command efficientlyPreWarmShootAndDrive(String pathName, String returnPa idle(), runOnce(() -> mRobotState.setUseAutonFallback(true)), mRobotState::hasNote))) - .andThen(either(driveToNextPiece(nextNote).withTimeout(3), none(), mRobotState::getUseAutonFallback)); + .andThen(either(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback)); } private Command followChoreoPathAndSeekNote(String pathName) { @@ -266,12 +283,13 @@ private Command followChoreoPathAndSeekNote(String pathName) { mPathBuilder.addPath(trajectory.getPoses()); var eventMarkers = ChoreoEventMarker.loadFromFile(pathName); - return new DriveAndSeekNote( + return clearNoteToSeek() + .andThen(new DriveAndSeekNote( mDrive, mRobotState, new DriveChoreoTrajectory(mDrive, mRobotState, trajectory, eventMarkers, false), mAutoNoteSeeker::getDetectedNoteToSeek, - kAutoTrapezoidalKinematicLimits) + kAutoTrapezoidalKinematicLimits)) .deadlineWith(index(), idleShooter()) .finallyDo(mAutoNoteSeeker::clear) .withName("FollowChoreoAndSeekNote"); diff --git a/src/main/java/com/team1701/robot/commands/ShootCommands.java b/src/main/java/com/team1701/robot/commands/ShootCommands.java index b11e9cff..a3f71fca 100644 --- a/src/main/java/com/team1701/robot/commands/ShootCommands.java +++ b/src/main/java/com/team1701/robot/commands/ShootCommands.java @@ -19,9 +19,10 @@ public class ShootCommands { public static Command idleShooterCommand(Shooter shooter, RobotState robotState) { return Commands.runEnd( - () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)), - shooter::stopRotation, - shooter); + () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)), + shooter::stopRotation, + shooter) + .withName("IdleShooterCommand"); } public static Command stop(Shooter shooter) { diff --git a/src/main/java/com/team1701/robot/simulation/NoteSimulator.java b/src/main/java/com/team1701/robot/simulation/NoteSimulator.java index 531493cf..457ba24e 100644 --- a/src/main/java/com/team1701/robot/simulation/NoteSimulator.java +++ b/src/main/java/com/team1701/robot/simulation/NoteSimulator.java @@ -55,8 +55,8 @@ public class NoteSimulator extends SubsystemBase { AutoNote.A.redPose(), AutoNote.B.redPose(), AutoNote.C.redPose(), - AutoNote.M1.pose(), - AutoNote.M2.pose(), + // AutoNote.M1.pose(), + // AutoNote.M2.pose(), AutoNote.M3.pose(), AutoNote.M4.pose(), AutoNote.M5.pose()) diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java index ccd622e9..817643de 100644 --- a/src/main/java/com/team1701/robot/states/RobotState.java +++ b/src/main/java/com/team1701/robot/states/RobotState.java @@ -279,8 +279,8 @@ public Rotation2d getToleranceSpeakerHeading(Translation2d translation) { .minus(translation) .getAngle(); - var toleranceRadians = Math.abs( - MathUtil.angleModulus(heading.getRadians() - getSpeakerHeading().getRadians())); + var toleranceRadians = Math.abs(MathUtil.angleModulus(heading.getRadians() + - FieldUtil.getHeadingToSpeaker(translation).getRadians())); return Rotation2d.fromRadians(Math.max(0.017, toleranceRadians / 2)); }