Skip to content

Commit 37cc387

Browse files
davidchen20davidchen20
committed
note filtering put in drive.java
1 parent 7e4a046 commit 37cc387

4 files changed

Lines changed: 78 additions & 151 deletions

File tree

‎src/main/java/frc/robot/BuildConstants.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,12 @@ public final class BuildConstants {
55
public static final String MAVEN_GROUP = "";
66
public static final String MAVEN_NAME = "2024RobotCode";
77
public static final String VERSION = "unspecified";
8-
public static final int GIT_REVISION = 318;
9-
public static final String GIT_SHA = "b2176b8c5ef241eedb55dea925bbab869219b7ca";
10-
public static final String GIT_DATE = "2024-08-14 20:36:51 EDT";
8+
public static final int GIT_REVISION = 320;
9+
public static final String GIT_SHA = "7e4a0469a390a05483a79d80ab72f641679887d4";
10+
public static final String GIT_DATE = "2024-08-16 13:52:21 EDT";
1111
public static final String GIT_BRANCH = "note-filtering";
12-
public static final String BUILD_DATE = "2024-08-14 21:54:44 EDT";
13-
public static final long BUILD_UNIX_TIME = 1723686884925L;
12+
public static final String BUILD_DATE = "2024-08-16 14:12:54 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723831974633L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 8 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -475,22 +475,14 @@ public RobotContainer() {
475475
.andThen(new InstantCommand(() -> shooter.stopFeeders())))
476476
.withTimeout(2.5));
477477

478-
NamedCommands.registerCommand(
479-
"C5", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.C5)));
480-
NamedCommands.registerCommand(
481-
"C4", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.C4)));
482-
NamedCommands.registerCommand(
483-
"C3", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.C3)));
484-
NamedCommands.registerCommand(
485-
"C2", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.C2)));
486-
NamedCommands.registerCommand(
487-
"C1", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.C1)));
488-
NamedCommands.registerCommand(
489-
"B1", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.B1)));
490-
NamedCommands.registerCommand(
491-
"B2", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.B2)));
492-
NamedCommands.registerCommand(
493-
"B3", new InstantCommand(() -> drive.setTargetNote(NOTE_POSITIONS.B3)));
478+
NamedCommands.registerCommand("C5", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.C5)));
479+
NamedCommands.registerCommand("C4", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.C4)));
480+
NamedCommands.registerCommand("C3", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.C3)));
481+
NamedCommands.registerCommand("C2", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.C2)));
482+
NamedCommands.registerCommand("C1", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.C1)));
483+
NamedCommands.registerCommand("B1", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.B1)));
484+
NamedCommands.registerCommand("B2", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.B2)));
485+
NamedCommands.registerCommand("B3", new InstantCommand(() -> drive.setNote(NOTE_POSITIONS.B3)));
494486

495487
// AUTO AIM COMMANDS
496488
NamedCommands.registerCommand("TurnToSpeaker", new TurnToSpeaker(drive, driveController));

‎src/main/java/frc/robot/commands/AlignToNoteAuto.java‎

Lines changed: 8 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -5,21 +5,16 @@
55
package frc.robot.commands;
66

77
import com.pathplanner.lib.auto.AutoBuilder;
8-
import edu.wpi.first.math.geometry.Rotation2d;
98
import edu.wpi.first.math.geometry.Translation2d;
109
import edu.wpi.first.wpilibj2.command.Command;
1110
import frc.robot.Constants;
1211
import frc.robot.Constants.LED_STATE;
13-
import frc.robot.Constants.NOTE_POSITIONS;
1412
import frc.robot.Constants.NoteState;
1513
import frc.robot.subsystems.drive.Drive;
1614
import frc.robot.subsystems.intake.Intake;
1715
import frc.robot.subsystems.led.LED;
1816
import frc.robot.subsystems.pivot.Pivot;
1917
import frc.robot.subsystems.shooter.Shooter;
20-
import frc.robot.util.AllianceFlipUtil;
21-
import frc.robot.util.FieldConstants;
22-
import java.util.HashMap;
2318
import org.littletonrobotics.junction.Logger;
2419

2520
public class AlignToNoteAuto extends Command {
@@ -31,14 +26,9 @@ public class AlignToNoteAuto extends Command {
3126
Intake intake;
3227
Shooter shooter;
3328
Command generatedPathCommand;
34-
Command targetNotePathCommand;
3529
Translation2d targetNoteLocation;
36-
Rotation2d targetNoteRotation;
37-
38-
private boolean useGeneratedPathCommand;
3930

4031
private boolean finished;
41-
private HashMap<NOTE_POSITIONS, Translation2d> noteLocations = new HashMap<>();
4232

4333
public AlignToNoteAuto(LED led, Drive drive, Shooter shooter, Intake intake, Pivot pivot) {
4434
this.shooter = shooter;
@@ -48,20 +38,6 @@ public AlignToNoteAuto(LED led, Drive drive, Shooter shooter, Intake intake, Piv
4838
this.drive = drive;
4939
finished = false;
5040

51-
noteLocations.put(NOTE_POSITIONS.C5, FieldConstants.StagingLocations.centerlineTranslations[0]);
52-
noteLocations.put(NOTE_POSITIONS.C4, FieldConstants.StagingLocations.centerlineTranslations[1]);
53-
noteLocations.put(NOTE_POSITIONS.C3, FieldConstants.StagingLocations.centerlineTranslations[2]);
54-
noteLocations.put(NOTE_POSITIONS.C2, FieldConstants.StagingLocations.centerlineTranslations[3]);
55-
noteLocations.put(NOTE_POSITIONS.C1, FieldConstants.StagingLocations.centerlineTranslations[4]);
56-
noteLocations.put(
57-
NOTE_POSITIONS.B1,
58-
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[2]));
59-
noteLocations.put(
60-
NOTE_POSITIONS.B2,
61-
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[1]));
62-
noteLocations.put(
63-
NOTE_POSITIONS.B3,
64-
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[0]));
6541
// Use addRequirements() here to declare subsystem dependencies.
6642
addRequirements(drive, shooter, led);
6743
}
@@ -75,38 +51,22 @@ public void initialize() {
7551
intake.runRollers(12);
7652
shooter.setFeedersRPM(500);
7753
pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
78-
targetNoteLocation = noteLocations.get(drive.getTargetNote());
79-
useGeneratedPathCommand =
80-
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 1.25
81-
&& drive.getCachedNoteLocation() != null;
82-
Logger.recordOutput(
83-
"cached note distance ", drive.getCachedNoteLocation().getDistance(targetNoteLocation));
84-
Logger.recordOutput("useGeneratedPath command", useGeneratedPathCommand);
85-
// useGeneratedPathCommand = false;
86-
// generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
87-
if (useGeneratedPathCommand) {
88-
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
89-
90-
generatedPathCommand.initialize();
91-
} else {
92-
targetNotePathCommand =
54+
targetNoteLocation = drive.getTargetNoteLocation();
55+
56+
generatedPathCommand =
9357
AutoBuilder.followPath(
94-
drive.generateTrajectory(targetNoteLocation, 3, 2.45, 100, 180, 0.5));
58+
drive.generateTrajectoryToNote(targetNoteLocation, 3, 2.45, 100, 180, 0.5));
9559

96-
targetNotePathCommand.initialize();
97-
}
60+
generatedPathCommand.initialize();
9861
}
9962

10063
// Called every time the scheduler runs while the command is scheduled.
10164
@Override
10265
public void execute() {
103-
Logger.recordOutput("useGeneratedPathCommand", useGeneratedPathCommand);
66+
10467
finished = shooter.seesNote() == NoteState.SENSOR;
105-
if (useGeneratedPathCommand) {
106-
generatedPathCommand.execute();
107-
} else {
108-
targetNotePathCommand.execute();
109-
}
68+
69+
generatedPathCommand.execute();
11070

11171
Logger.recordOutput("path is finished", finished);
11272
}
@@ -117,7 +77,6 @@ public void end(boolean interrupted) {
11777
led.setState(LED_STATE.BLUE);
11878
intake.stopRollers();
11979
shooter.stopFeeders();
120-
// pathCommand.cancel();
12180
}
12281

12382
// Returns true when the command should end.

‎src/main/java/frc/robot/subsystems/drive/Drive.java‎

Lines changed: 57 additions & 81 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@
5959
import frc.robot.util.LimelightHelpers.PoseEstimate;
6060
import frc.robot.util.LimelightHelpers.RawFiducial;
6161
import frc.robot.util.LocalADStarAK;
62+
import java.util.HashMap;
6263
import java.util.List;
6364
import java.util.Optional;
6465
import org.littletonrobotics.junction.AutoLogOutput;
@@ -120,6 +121,8 @@ public TimestampedT2d(Translation2d translation, double time) {
120121

121122
CircularBuffer<TimestampedPose2d> robotPoseBuffer;
122123

124+
private HashMap<NOTE_POSITIONS, Translation2d> noteLocations = new HashMap<>();
125+
123126
public Drive(
124127
GyroIO gyroIO,
125128
VisionIO visionIO,
@@ -185,6 +188,21 @@ public Drive(
185188
rotationController.setTolerance(5);
186189
rotationController.enableContinuousInput(-180, 180);
187190
robotPoseBuffer = new CircularBuffer<>(11);
191+
192+
noteLocations.put(NOTE_POSITIONS.C5, FieldConstants.StagingLocations.centerlineTranslations[0]);
193+
noteLocations.put(NOTE_POSITIONS.C4, FieldConstants.StagingLocations.centerlineTranslations[1]);
194+
noteLocations.put(NOTE_POSITIONS.C3, FieldConstants.StagingLocations.centerlineTranslations[2]);
195+
noteLocations.put(NOTE_POSITIONS.C2, FieldConstants.StagingLocations.centerlineTranslations[3]);
196+
noteLocations.put(NOTE_POSITIONS.C1, FieldConstants.StagingLocations.centerlineTranslations[4]);
197+
noteLocations.put(
198+
NOTE_POSITIONS.B1,
199+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[2]));
200+
noteLocations.put(
201+
NOTE_POSITIONS.B2,
202+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[1]));
203+
noteLocations.put(
204+
NOTE_POSITIONS.B3,
205+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[0]));
188206
}
189207

190208
public void periodic() {
@@ -698,7 +716,37 @@ public Optional<Rotation2d> turnToSpeakerAngle() {
698716
return Optional.empty();
699717
}
700718

701-
public PathPlannerPath generateTrajectory(
719+
public void runPath(PathPlannerPath path) {
720+
AutoBuilder.followPath(path).schedule();
721+
}
722+
723+
public Translation2d getTargetNoteLocation() {
724+
if (visionInputs.iTX != 0.0) {
725+
double taThreshold = 0;
726+
if (visionInputs.iTA >= taThreshold) {
727+
lastNoteLocT2d.translation =
728+
calculateNotePositionFieldRelative().getTranslation().toTranslation2d();
729+
lastNoteLocT2d.time = Timer.getFPGATimestamp();
730+
}
731+
}
732+
733+
Translation2d visionCoords = getCachedNoteLocation();
734+
Translation2d fieldCoords = noteLocations.get(getNote());
735+
boolean useVisionNoteCoords =
736+
getCachedNoteLocation().getDistance(fieldCoords) < 1.25
737+
&& getCachedNoteLocation() != null
738+
&& getCachedNoteTime() != -1
739+
&& noteImageIsNew();
740+
741+
Logger.recordOutput(
742+
"cached note distance to field ", getCachedNoteLocation().getDistance(fieldCoords));
743+
Logger.recordOutput("use vision note coords", useVisionNoteCoords);
744+
745+
if (useVisionNoteCoords) return visionCoords;
746+
return fieldCoords;
747+
}
748+
749+
public PathPlannerPath generateTrajectoryToNote(
702750
Translation2d target,
703751
double maxVelMetersPerSec,
704752
double maxAccelMetersPerSecSquared,
@@ -707,10 +755,12 @@ public PathPlannerPath generateTrajectory(
707755
double endVelMetersPerSec) {
708756
Rotation2d targetRotation =
709757
new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY());
758+
759+
Logger.recordOutput("Target Note Pose3d", new Pose3d(new Pose2d(target, new Rotation2d())));
710760
List<Translation2d> points =
711761
PathPlannerPath.bezierFromPoses(
712762
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()),
713-
new Pose2d(target.getX(), target.getY(), AllianceFlipUtil.apply(targetRotation)));
763+
new Pose2d(target.getX(), target.getY(), targetRotation));
714764
PathPlannerPath path =
715765
new PathPlannerPath(
716766
points,
@@ -719,25 +769,14 @@ public PathPlannerPath generateTrajectory(
719769
maxAccelMetersPerSecSquared,
720770
Units.degreesToRadians(maxAngVelDegPerSec),
721771
Units.degreesToRadians(maxAngAccelDegPerSecSquared)),
722-
new GoalEndState(endVelMetersPerSec, AllianceFlipUtil.apply(targetRotation), true));
772+
new GoalEndState(endVelMetersPerSec, targetRotation, true));
723773

724-
return path;
725-
}
774+
path.preventFlipping = true;
726775

727-
public void runPath(PathPlannerPath path) {
728-
AutoBuilder.followPath(path).schedule();
776+
return path;
729777
}
730778

731779
public PathPlannerPath generatePathToNote() {
732-
if (visionInputs.iTX != 0.0) {
733-
double taThreshold = 0;
734-
if (visionInputs.iTA >= taThreshold) {
735-
lastNoteLocT2d.translation =
736-
calculateNotePositionFieldRelative().getTranslation().toTranslation2d();
737-
lastNoteLocT2d.time = Timer.getFPGATimestamp();
738-
}
739-
}
740-
741780
Rotation2d targetRotation;
742781
Logger.recordOutput("note timeess", getCachedNoteTime());
743782
if (getCachedNoteTime() != -1) {
@@ -800,69 +839,6 @@ public PathPlannerPath generatePathToNote() {
800839
}
801840
}
802841

803-
public PathPlannerPath generatePathToNoteBlind(Translation2d targetNoteLocation) {
804-
// if (visionInputs.iTX != 0.0) {
805-
// double taThreshold = 0;
806-
// if (visionInputs.iTA >= taThreshold) {
807-
// lastNoteLocT2d.translation =
808-
// calculateNotePositionFieldRelative().getTranslation().toTranslation2d();
809-
// lastNoteLocT2d.time = Timer.getFPGATimestamp();
810-
// }
811-
// }
812-
// Pose2d targetNoteLocation = noteLocations.get(drive.getTargetNote());
813-
814-
Rotation2d targetRotation;
815-
Logger.recordOutput("note timeess", getCachedNoteTime());
816-
// led.setState(LED_STATE.FLASHING_RED);
817-
// Translation2d targetNoteLocation = getCachedNoteLocation();
818-
Logger.recordOutput("better translate", targetNoteLocation);
819-
if (noteImageIsNew()) {
820-
821-
targetRotation =
822-
new Rotation2d(
823-
targetNoteLocation.getX() - getPose().getX(),
824-
targetNoteLocation.getY() - getPose().getY());
825-
List<Translation2d> pointsToNote;
826-
if (DriverStation.getAlliance().get().equals(Alliance.Blue)) {
827-
Logger.recordOutput(
828-
"goal point blue",
829-
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
830-
pointsToNote =
831-
PathPlannerPath.bezierFromPoses(
832-
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
833-
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
834-
} else {
835-
Logger.recordOutput(
836-
"goal point red",
837-
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
838-
pointsToNote =
839-
PathPlannerPath.bezierFromPoses(
840-
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
841-
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
842-
}
843-
PathPlannerPath path =
844-
new PathPlannerPath(
845-
pointsToNote,
846-
new PathConstraints(
847-
3, 2.45, Units.degreesToRadians(100), Units.degreesToRadians(180)),
848-
new GoalEndState(0.5, targetRotation, true));
849-
850-
path.preventFlipping = true;
851-
// AutoBuilder.followPath(path).schedule();
852-
Logger.recordOutput("follow path", true);
853-
return path;
854-
} else {
855-
// return;
856-
// led.setState(LED_STATE.PAPAYA_ORANGE);
857-
return new PathPlannerPath(
858-
PathPlannerPath.bezierFromPoses(
859-
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()),
860-
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation())),
861-
new PathConstraints(0.1, 1.5, Units.degreesToRadians(100), Units.degreesToRadians(180)),
862-
new GoalEndState(0.5, getPose().getRotation(), true));
863-
}
864-
}
865-
866842
public Command createPathFindingCommand(Pose2d target) {
867843
Pose2d coord = target;
868844
PathConstraints constraints =
@@ -1019,11 +995,11 @@ public Command followPathCommand(String pathName, boolean lowerPID) {
1019995
);
1020996
}
1021997

1022-
public void setTargetNote(NOTE_POSITIONS targetNote) {
998+
public void setNote(NOTE_POSITIONS targetNote) {
1023999
this.targetNote = targetNote;
10241000
}
10251001

1026-
public NOTE_POSITIONS getTargetNote() {
1002+
public NOTE_POSITIONS getNote() {
10271003
return targetNote;
10281004
}
10291005
}

0 commit comments

Comments
 (0)