Skip to content

Commit be16f2e

Browse files
BenG49BenG49
authored
April 8 (#103)
* Remove unused autos * Make four piece from old 5 piece * kA and FOC all the time * Align and shoot without stopping in teleop * Four piece * Make 5 piece use old first shot, save old 5 piece * Put shoot time on preload six piece back to more normal * Make red 4 piece CBA
1 parent 4290558 commit be16f2e

File tree

12 files changed

+278
-167
lines changed

12 files changed

+278
-167
lines changed

‎src/main/deploy/pathplanner/autos/5 CBAE Podium Forward.auto

Lines changed: 0 additions & 43 deletions
This file was deleted.

‎src/main/java/com/stuypulse/robot/RobotContainer.java

Lines changed: 12 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -147,13 +147,8 @@ private void configureDriverBindings() {
147147
// then shoot
148148
driver.getRightBumper()
149149
.onTrue(new ShooterPodiumShot())
150-
.whileTrue(new SwerveDriveToShoot()
151-
.deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN))
152-
.andThen(new ShooterWaitForTarget()
153-
.withTimeout(1.5))
154-
.andThen(new ConveyorShoot()))
155-
.onFalse(new ConveyorStop())
156-
.onFalse(new IntakeStop());
150+
.whileTrue(new SwerveDriveToShootWithoutStopping()
151+
.deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)));
157152

158153
// note to amper and align then score
159154
driver.getLeftBumper()
@@ -216,7 +211,6 @@ private void configureDriverBindings() {
216211

217212
driver.getTopButton()
218213
.whileTrue(new SwerveDriveAutoFerry(driver));
219-
// .whileTrue(new SwerveDriveToShootMoving());
220214

221215
// climb
222216
driver.getRightButton()
@@ -318,16 +312,15 @@ public void configureAutons() {
318312
AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new,
319313
"Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)");
320314

321-
AutonConfig TrackingCBAE = new AutonConfig("Tracking 5 CBAE Podium", FivePieceTrackingCBAE::new,
322-
"Preload to C", "C to B", "B to A", "A to E", "E to Shoot");
323-
324315
AutonConfig CBAED = new AutonConfig("5 CBAE", SixPieceCBAED::new,
325-
"Preload to C Close", "Close Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot");
316+
"Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot");
326317
AutonConfig CBAED_RED = new AutonConfig("5 CBAE", SixPieceCBAED::new,
327-
"Preload to C Close Red", "Close Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot");
318+
"Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot");
328319

329-
AutonConfig CBAED_OLD = new AutonConfig("5 CBAE Old", SixPieceCBAEDOld::new,
330-
"Preload to C", "C to B", "B to A", "A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot");
320+
AutonConfig CBA = new AutonConfig("4 CBA", FourPieceCBA::new,
321+
"Preload to C", "C to B", "B to A");
322+
AutonConfig CBA_RED = new AutonConfig("4 CBA", FourPieceCBA::new,
323+
"Preload to C", "C to B Red", "B to A Red");
331324

332325
AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new,
333326
"Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)");
@@ -340,16 +333,16 @@ public void configureAutons() {
340333

341334
// AutonConfig DE = new AutonConfig("2 DE", TwoPieceDE::new,
342335
// "Preload Shot to D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot");
343-
344-
// AutonConfig PodiumCloseCBAE = new AutonConfig("Podium Close 5 Piece CBAE", FivePiecePodiumForwardCBAE::new,
345-
// "Forward First Piece to C", "C to B 2", "B to A","A to E", "E to Shoot");
346336

347337
// TODO: automatically choose red/blue
348338
HGF.registerDefaultBlue(autonChooser);
349339
HGF_RED.registerRed(autonChooser);
350-
340+
351341
CBAED.registerBlue(autonChooser);
352342
CBAED_RED.registerRed(autonChooser);
343+
344+
CBA.registerBlue(autonChooser);
345+
CBA_RED.registerRed(autonChooser);
353346

354347
SmartDashboard.putData("Autonomous", autonChooser);
355348

‎src/main/java/com/stuypulse/robot/commands/FastAlignShootSpeakerRelative.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ public FastAlignShootSpeakerRelative(double angleToSpeaker, double distanceToSpe
111111
xTolerance = 0.2;
112112
yTolerance = 0.2;
113113
thetaTolerance = 10;
114-
velocityTolerance = 0.2;
114+
velocityTolerance = 0.1;
115115

116116
addRequirements(swerve);
117117
}
@@ -150,6 +150,8 @@ public void execute() {
150150
conveyor.toShooter();
151151
intake.acquire();
152152
}
153+
154+
SmartDashboard.putNumber("Alignment/Output Speed", speed.magnitude());
153155
}
154156

155157
@Override

‎src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceTrackingCBAE.java

Lines changed: 0 additions & 44 deletions
This file was deleted.

‎src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBA.java

Lines changed: 19 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
package com.stuypulse.robot.commands.auton.CBADE;
22

3-
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
3+
import com.pathplanner.lib.path.PathPlannerPath;
44
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
55
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
66
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
@@ -12,30 +12,36 @@
1212
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
1313
import edu.wpi.first.wpilibj2.command.WaitCommand;
1414

15+
/**
16+
* Shoots second shot at podium, shoots first shot between C and B
17+
*/
1518
public class FourPieceCBA extends SequentialCommandGroup {
1619

17-
public FourPieceCBA() {
20+
public FourPieceCBA(PathPlannerPath... paths) {
1821
addCommands(
1922
new ParallelCommandGroup(
2023
new WaitCommand(Auton.SHOOTER_STARTUP_DELAY)
2124
.andThen(new ShooterPodiumShot()),
2225

23-
SwerveDriveToPose.speakerRelative(-45)
26+
SwerveDriveToPose.speakerRelative(-15)
27+
.withTolerance(0.03, 0.03, 3)
2428
),
2529

26-
new ConveyorShootRoutine(),
30+
new ConveyorShootRoutine(0.8),
2731

28-
// new FollowPathAndIntake("First Piece to C"),
29-
// new SwerveDriveToShoot(2.9),
30-
// new ConveyorShootRoutine(),
32+
new FollowPathAndIntake(paths[0]),
33+
new SwerveDriveToShoot()
34+
.withTolerance(0.03, 3),
35+
new ConveyorShootRoutine(),
3136

32-
// new FollowPathAndIntake("C to B"),
33-
// new SwerveDriveToShoot(),
34-
// new ConveyorShootRoutine(),
37+
new FollowPathAndIntake(paths[1]),
38+
new SwerveDriveToShoot()
39+
.withTolerance(0.03, 3),
40+
new ConveyorShootRoutine(),
3541

36-
// new FollowPathAndIntake("B to A"),
37-
new SwerveDriveToShoot(2.9)
38-
.withTolerance(0.05, 3),
42+
new FollowPathAndIntake(paths[2]),
43+
new SwerveDriveToShoot()
44+
.withTolerance(0.03, 3),
3945
new ConveyorShootRoutine()
4046
);
4147
}

‎src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java

Lines changed: 12 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,12 @@
33
import com.pathplanner.lib.path.PathPlannerPath;
44
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
55
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
6-
import com.stuypulse.robot.commands.conveyor.ConveyorShootNoIntake;
76
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
8-
import com.stuypulse.robot.commands.conveyor.ConveyorStop;
97
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
108
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
119
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
1210
import com.stuypulse.robot.constants.Settings.Auton;
13-
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
1411

15-
import edu.wpi.first.wpilibj2.command.InstantCommand;
1612
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
1713
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
1814
import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -26,35 +22,33 @@ public SixPieceCBAED(PathPlannerPath... paths) {
2622
new WaitCommand(Auton.SHOOTER_STARTUP_DELAY)
2723
.andThen(new ShooterPodiumShot()),
2824

29-
SwerveDrive.getInstance().followPathCommand(paths[0])
25+
SwerveDriveToPose.speakerRelative(-15)
26+
.withTolerance(0.03, 0.03, 3)
3027
),
3128

32-
new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()),
33-
new ConveyorShootNoIntake(),
34-
new WaitCommand(0.55),
35-
new ConveyorStop(),
29+
new ConveyorShootRoutine(0.55),
3630

37-
new FollowPathAndIntake(paths[1]),
31+
new FollowPathAndIntake(paths[0]),
3832
new SwerveDriveToShoot()
39-
.withTolerance(0.05, 7),
33+
.withTolerance(0.03, 7),
4034
new ConveyorShootRoutine(),
4135

42-
new FollowPathAndIntake(paths[2]),
36+
new FollowPathAndIntake(paths[1]),
4337
new SwerveDriveToShoot()
4438
.withTolerance(0.05, 7),
4539
new ConveyorShootRoutine(),
4640

47-
new FollowPathAndIntake(paths[3]),
41+
new FollowPathAndIntake(paths[2]),
4842
new SwerveDriveToShoot()
4943
.withTolerance(0.05, 5),
50-
new ConveyorShootRoutine(),
44+
new ConveyorShootRoutine(0.6),
5145

52-
new FollowPathAndIntake(paths[4]),
53-
new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()
46+
new FollowPathAndIntake(paths[3]),
47+
new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()
5448
.withTolerance(0.033, 7)),
5549

56-
new FollowPathAndIntake(paths[6]),
57-
new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()
50+
new FollowPathAndIntake(paths[5]),
51+
new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()
5852
.withTolerance(0.033, 7))
5953
);
6054
}

‎src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAEDOld.java

Lines changed: 28 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -3,51 +3,59 @@
33
import com.pathplanner.lib.path.PathPlannerPath;
44
import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot;
55
import com.stuypulse.robot.commands.auton.FollowPathAndIntake;
6+
import com.stuypulse.robot.commands.conveyor.ConveyorShootNoIntake;
67
import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine;
8+
import com.stuypulse.robot.commands.conveyor.ConveyorStop;
79
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
8-
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
910
import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot;
1011
import com.stuypulse.robot.constants.Settings.Auton;
12+
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
1113

1214
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
1315
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
1416
import edu.wpi.first.wpilibj2.command.WaitCommand;
1517

16-
/**
17-
* Shoots second shot at podium, shoots first shot between C and B
18-
*/
1918
public class SixPieceCBAEDOld extends SequentialCommandGroup {
20-
19+
20+
2121
public SixPieceCBAEDOld(PathPlannerPath... paths) {
2222
addCommands(
2323
new ParallelCommandGroup(
2424
new WaitCommand(Auton.SHOOTER_STARTUP_DELAY)
2525
.andThen(new ShooterPodiumShot()),
26-
27-
SwerveDriveToPose.speakerRelative(-15)
28-
.withTolerance(0.1, 0.1, 3)
26+
27+
SwerveDrive.getInstance().followPathCommand(paths[0])
2928
),
3029

31-
new ConveyorShootRoutine(),
32-
33-
new FollowPathAndIntake(paths[0]),
34-
new SwerveDriveToShoot(),
35-
new ConveyorShootRoutine(),
30+
new SwerveDriveToShoot()
31+
.withTolerance(100, 5),
32+
new ConveyorShootNoIntake(),
33+
new WaitCommand(0.55),
34+
new ConveyorStop(),
3635

3736
new FollowPathAndIntake(paths[1]),
38-
new SwerveDriveToShoot(),
37+
new SwerveDriveToShoot()
38+
.withTolerance(0.03, 7),
3939
new ConveyorShootRoutine(),
4040

4141
new FollowPathAndIntake(paths[2]),
42-
new SwerveDriveToShoot(),
42+
new SwerveDriveToShoot()
43+
.withTolerance(0.05, 7),
4344
new ConveyorShootRoutine(),
4445

4546
new FollowPathAndIntake(paths[3]),
46-
new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()),
47-
48-
new FollowPathAndIntake(paths[5]),
49-
new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot())
47+
new SwerveDriveToShoot()
48+
.withTolerance(0.05, 5),
49+
new ConveyorShootRoutine(0.6),
50+
51+
new FollowPathAndIntake(paths[4]),
52+
new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()
53+
.withTolerance(0.033, 7)),
54+
55+
new FollowPathAndIntake(paths[6]),
56+
new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()
57+
.withTolerance(0.033, 7))
5058
);
5159
}
52-
60+
5361
}

‎src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@
88
import com.stuypulse.robot.commands.shooter.ShooterPodiumShot;
99
import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget;
1010
import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
11-
import com.stuypulse.robot.constants.Settings.Auton;
1211

1312
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
1413
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -22,7 +21,7 @@ public FourPieceHGF(PathPlannerPath... paths) {
2221
new WaitCommand(0.25)
2322
.andThen(new ShooterPodiumShot()),
2423

25-
SwerveDriveToPose.speakerRelative(-45)
24+
SwerveDriveToPose.speakerRelative(-55)
2625
.withTolerance(0.1, 0.1, 2)
2726
),
2827

0 commit comments

Comments
 (0)