Skip to content

Commit 07bbefc

Browse files
davidchen20davidchen20
committed
note filtering
1 parent dbfb048 commit 07bbefc

9 files changed

Lines changed: 149 additions & 22 deletions

File tree

‎src/main/deploy/pathplanner/autos/$c!p-b3-b2-b1.auto‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,12 @@
4848
"pathName": "$cpb(321)!b3"
4949
}
5050
},
51+
{
52+
"type": "named",
53+
"data": {
54+
"name": "B3"
55+
}
56+
},
5157
{
5258
"type": "named",
5359
"data": {
@@ -72,6 +78,12 @@
7278
"waitTime": 0.3
7379
}
7480
},
81+
{
82+
"type": "named",
83+
"data": {
84+
"name": "B2"
85+
}
86+
},
7587
{
7688
"type": "named",
7789
"data": {
@@ -90,6 +102,12 @@
90102
"pathName": "$cpb(321)!b1"
91103
}
92104
},
105+
{
106+
"type": "named",
107+
"data": {
108+
"name": "B1"
109+
}
110+
},
93111
{
94112
"type": "named",
95113
"data": {
@@ -114,6 +132,12 @@
114132
"pathName": "gasgas"
115133
}
116134
},
135+
{
136+
"type": "named",
137+
"data": {
138+
"name": "C1"
139+
}
140+
},
117141
{
118142
"type": "named",
119143
"data": {

‎src/main/deploy/pathplanner/autos/$s!p-c5-c4.auto‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,12 @@
2323
"pathName": "$s!p-c5"
2424
}
2525
},
26+
{
27+
"type": "named",
28+
"data": {
29+
"name": "C5"
30+
}
31+
},
2632
{
2733
"type": "named",
2834
"data": {
@@ -53,6 +59,12 @@
5359
"pathName": "$s!c4"
5460
}
5561
},
62+
{
63+
"type": "named",
64+
"data": {
65+
"name": "C4"
66+
}
67+
},
5668
{
5769
"type": "named",
5870
"data": {

‎src/main/deploy/pathplanner/paths/New New New New New Path.path‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,12 @@
1616
},
1717
{
1818
"anchor": {
19-
"x": 4.0,
20-
"y": 6.0
19+
"x": 8.317099886696207,
20+
"y": 7.424670370025476
2121
},
2222
"prevControl": {
23-
"x": 3.0,
24-
"y": 6.0
23+
"x": 7.317099886696207,
24+
"y": 7.424670370025476
2525
},
2626
"nextControl": null,
2727
"isLocked": false,

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

Lines changed: 6 additions & 6 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 = 313;
9-
public static final String GIT_SHA = "974424bf6ad82162eaa1ac360b41f2aa0d05c04d";
10-
public static final String GIT_DATE = "2024-07-27 20:08:15 EDT";
11-
public static final String GIT_BRANCH = "rainbow-rumble";
12-
public static final String BUILD_DATE = "2024-07-28 13:00:32 EDT";
13-
public static final long BUILD_UNIX_TIME = 1722186032402L;
8+
public static final int GIT_REVISION = 314;
9+
public static final String GIT_SHA = "dbfb048d3f7aa479526221e9947ca94220ee5472";
10+
public static final String GIT_DATE = "2024-07-28 19:57:18 EDT";
11+
public static final String GIT_BRANCH = "note-filtering";
12+
public static final String BUILD_DATE = "2024-08-02 11:57:52 EDT";
13+
public static final long BUILD_UNIX_TIME = 1722614272911L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

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

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ public static Mode getMode() {
3838
};
3939
}
4040

41-
public static final Mode currentMode = Mode.REAL;
41+
public static final Mode currentMode = Mode.SIM;
4242
public static final boolean tuningMode = true;
4343
public static final String CANBUS = "CAN Bus 2";
4444
public static final double LOOP_PERIOD_SECS = 0.02;
@@ -146,6 +146,17 @@ public static class LEDConstants {
146146
public static final double COLOR_VIOLET = 0.91;
147147
}
148148

149+
public static enum NOTE_POSITIONS {
150+
B1,
151+
B2,
152+
B3,
153+
C1,
154+
C2,
155+
C3,
156+
C4,
157+
C5
158+
}
159+
149160
public static enum LED_STATE {
150161
BLUE,
151162
RED,

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

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
3333
import edu.wpi.first.wpilibj2.command.button.Trigger;
3434
import frc.robot.Constants.LED_STATE;
35+
import frc.robot.Constants.NOTE_POSITIONS;
3536
import frc.robot.Constants.NoteState;
3637
import frc.robot.Constants.SHOOT_STATE;
3738
import frc.robot.commands.AimbotAuto;
@@ -474,6 +475,23 @@ public RobotContainer() {
474475
.andThen(new InstantCommand(() -> shooter.stopFeeders())))
475476
.withTimeout(2.5));
476477

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)));
494+
477495
// AUTO AIM COMMANDS
478496
NamedCommands.registerCommand("TurnToSpeaker", new TurnToSpeaker(drive, driveController));
479497
NamedCommands.registerCommand("AngleShooter", new AngleShooter(drive, shooter, pivot));

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

Lines changed: 59 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -5,15 +5,22 @@
55
package frc.robot.commands;
66

77
import com.pathplanner.lib.auto.AutoBuilder;
8+
import edu.wpi.first.math.geometry.Pose2d;
9+
import edu.wpi.first.math.geometry.Rotation2d;
10+
import edu.wpi.first.math.geometry.Translation2d;
811
import edu.wpi.first.wpilibj2.command.Command;
912
import frc.robot.Constants;
1013
import frc.robot.Constants.LED_STATE;
14+
import frc.robot.Constants.NOTE_POSITIONS;
1115
import frc.robot.Constants.NoteState;
1216
import frc.robot.subsystems.drive.Drive;
1317
import frc.robot.subsystems.intake.Intake;
1418
import frc.robot.subsystems.led.LED;
1519
import frc.robot.subsystems.pivot.Pivot;
1620
import frc.robot.subsystems.shooter.Shooter;
21+
import frc.robot.util.AllianceFlipUtil;
22+
import frc.robot.util.FieldConstants;
23+
import java.util.HashMap;
1724
import org.littletonrobotics.junction.Logger;
1825

1926
public class AlignToNoteAuto extends Command {
@@ -24,9 +31,15 @@ public class AlignToNoteAuto extends Command {
2431
Pivot pivot;
2532
Intake intake;
2633
Shooter shooter;
27-
Command pathCommand;
34+
Command generatedPathCommand;
35+
Command targetNotePathCommand;
36+
Translation2d targetNoteLocation;
37+
Rotation2d targetNoteRotation;
38+
39+
private boolean useGeneratedPathCommand;
2840

2941
private boolean finished;
42+
private HashMap<NOTE_POSITIONS, Translation2d> noteLocations = new HashMap<>();
3043

3144
public AlignToNoteAuto(LED led, Drive drive, Shooter shooter, Intake intake, Pivot pivot) {
3245
this.shooter = shooter;
@@ -35,6 +48,21 @@ public AlignToNoteAuto(LED led, Drive drive, Shooter shooter, Intake intake, Piv
3548
this.led = led;
3649
this.drive = drive;
3750
finished = false;
51+
52+
noteLocations.put(NOTE_POSITIONS.C5, FieldConstants.StagingLocations.centerlineTranslations[0]);
53+
noteLocations.put(NOTE_POSITIONS.C4, FieldConstants.StagingLocations.centerlineTranslations[1]);
54+
noteLocations.put(NOTE_POSITIONS.C3, FieldConstants.StagingLocations.centerlineTranslations[2]);
55+
noteLocations.put(NOTE_POSITIONS.C2, FieldConstants.StagingLocations.centerlineTranslations[3]);
56+
noteLocations.put(NOTE_POSITIONS.C1, FieldConstants.StagingLocations.centerlineTranslations[4]);
57+
noteLocations.put(
58+
NOTE_POSITIONS.B1,
59+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[2]));
60+
noteLocations.put(
61+
NOTE_POSITIONS.B2,
62+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[1]));
63+
noteLocations.put(
64+
NOTE_POSITIONS.B3,
65+
AllianceFlipUtil.apply(FieldConstants.StagingLocations.spikeTranslations[0]));
3866
// Use addRequirements() here to declare subsystem dependencies.
3967
addRequirements(drive, shooter, led);
4068
}
@@ -43,22 +71,43 @@ public AlignToNoteAuto(LED led, Drive drive, Shooter shooter, Intake intake, Piv
4371
@Override
4472
public void initialize() {
4573
Logger.recordOutput("auto pickup init", "true");
74+
4675
led.setState(LED_STATE.FLASHING_RED);
4776
intake.runRollers(12);
4877
shooter.setFeedersRPM(500);
4978
pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
50-
// this.pathCommand = drive.alignToNote();
51-
// pathCommand.schedule();
52-
pathCommand = AutoBuilder.followPath(drive.generatePathToNote());
53-
pathCommand.initialize();
79+
targetNoteLocation = noteLocations.get(drive.getTargetNote());
80+
useGeneratedPathCommand =
81+
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 1
82+
|| drive.getCachedNoteLocation() != null;
83+
// useGeneratedPathCommand = false;
84+
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
85+
if (useGeneratedPathCommand) {
86+
87+
generatedPathCommand.initialize();
88+
} else {
89+
targetNoteRotation =
90+
new Rotation2d(
91+
targetNoteLocation.getX() - drive.getPose().getX(),
92+
targetNoteLocation.getY() - drive.getPose().getY());
93+
targetNotePathCommand =
94+
drive.generateTrajectory(
95+
new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
96+
targetNotePathCommand.initialize();
97+
}
5498
}
5599

56100
// Called every time the scheduler runs while the command is scheduled.
57101
@Override
58102
public void execute() {
59-
103+
Logger.recordOutput("useGeneratedPathCommand", useGeneratedPathCommand);
60104
finished = shooter.seesNote() == NoteState.SENSOR;
61-
pathCommand.execute();
105+
if (useGeneratedPathCommand) {
106+
generatedPathCommand.execute();
107+
} else {
108+
targetNotePathCommand.execute();
109+
}
110+
62111
Logger.recordOutput("path is finished", finished);
63112
}
64113

@@ -75,8 +124,9 @@ public void end(boolean interrupted) {
75124
@Override
76125
public boolean isFinished() {
77126
Logger.recordOutput("isFinished align note", shooter.seesNote());
127+
// return false;
78128
return shooter.seesNote() == NoteState.SENSOR
79-
|| shooter.seesNote() == NoteState.CURRENT
80-
|| finished;
129+
|| shooter.seesNote() == NoteState.CURRENT
130+
|| finished;
81131
}
82132
}

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

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,7 @@
5151
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
5252
import frc.robot.Constants;
5353
import frc.robot.Constants.LED_STATE;
54+
import frc.robot.Constants.NOTE_POSITIONS;
5455
import frc.robot.subsystems.led.LED;
5556
import frc.robot.util.FieldConstants;
5657
import frc.robot.util.LimelightHelpers;
@@ -71,6 +72,8 @@ public class Drive extends SubsystemBase {
7172
private static double multiplier = 1.0;
7273
private static boolean toggle = false;
7374

75+
private NOTE_POSITIONS targetNote;
76+
7477
private boolean overridePathplanner = false;
7578

7679
private NetworkTable limelightintake =
@@ -633,6 +636,7 @@ public Translation2d getCachedNoteLocation() {
633636
// return new Translation2d(0, 0);
634637
// } else {
635638
return lastNoteLocT2d.translation;
639+
// return new Translation2d(0, 0);
636640
// }
637641
}
638642

@@ -938,4 +942,12 @@ public Command followPathCommand(String pathName, boolean lowerPID) {
938942
this // Reference to this subsystem to set requirements
939943
);
940944
}
945+
946+
public void setTargetNote(NOTE_POSITIONS targetNote) {
947+
this.targetNote = targetNote;
948+
}
949+
950+
public NOTE_POSITIONS getTargetNote() {
951+
return targetNote;
952+
}
941953
}

‎src/main/java/frc/robot/util/FieldConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,13 +49,13 @@ public static final class StagingLocations {
4949
static {
5050
for (int i = 0; i < centerlineTranslations.length; i++) {
5151
centerlineTranslations[i] =
52-
new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY));
52+
new Translation2d(centerlineFirstY + (i * centerlineSeparationY), centerlineX);
5353
}
5454
}
5555

5656
static {
5757
for (int i = 0; i < spikeTranslations.length; i++) {
58-
spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY));
58+
spikeTranslations[i] = new Translation2d(spikeFirstY + (i * spikeSeparationY), spikeX);
5959
}
6060
}
6161
}

0 commit comments

Comments
 (0)