Skip to content

Commit 79b2c45

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #76 from strykeforce/worldsDrivePractice
Worlds drive practice
2 parents c308be2 + 1390924 commit 79b2c45

32 files changed

+1305
-472
lines changed

‎src/main/deploy/choreo/OppBargeToOppG.traj‎

Lines changed: 300 additions & 76 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/OppGToOppBarge.traj‎

Lines changed: 263 additions & 60 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/bargeToE.traj‎

Lines changed: 156 additions & 152 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/midStartPToESlow.traj‎

Lines changed: 112 additions & 0 deletions
Large diffs are not rendered by default.

‎src/main/deploy/choreo/midStartPtoESlow.traj‎

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

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

Lines changed: 24 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -461,29 +461,30 @@ private void configureOperatorBindings() {
461461

462462
new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem));
463463

464-
// Move biscuit
465-
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
466-
.onTrue(
467-
new JogBiscuitCommand(
468-
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
469-
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
470-
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
471-
.onTrue(
472-
new JogBiscuitCommand(
473-
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
474-
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
475-
476-
// Move elevator
477-
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
478-
.onTrue(
479-
new JogElevatorCommand(
480-
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
481-
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
482-
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
483-
.onTrue(
484-
new JogElevatorCommand(
485-
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
486-
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
464+
// // Move biscuit
465+
// new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
466+
// .onTrue(
467+
// new JogBiscuitCommand(
468+
// biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
469+
// .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
470+
// new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
471+
// .onTrue(
472+
// new JogBiscuitCommand(
473+
// biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
474+
// .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
475+
476+
// // Move elevator
477+
// new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
478+
// .onTrue(
479+
// new JogElevatorCommand(
480+
// elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
481+
// .onFalse(new HoldElevatorCommand(elevatorSubsystem));
482+
// new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
483+
// .onTrue(
484+
// new JogElevatorCommand(
485+
// elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown,
486+
// Rotations)))
487+
// .onFalse(new HoldElevatorCommand(elevatorSubsystem));
487488
}
488489

489490
private void configureTestOperatorBindings() {

‎src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,12 @@
44
import choreo.trajectory.SwerveSample;
55
import choreo.trajectory.Trajectory;
66
import edu.wpi.first.math.geometry.Pose2d;
7+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
78
import edu.wpi.first.wpilibj.Timer;
89
import edu.wpi.first.wpilibj2.command.Command;
910
import frc.robot.commands.drive.DriveAutonCommand;
1011
import frc.robot.constants.AutonConstants;
12+
import frc.robot.constants.DriveConstants;
1113
import frc.robot.constants.PathHandlerConstants;
1214
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
1315
import frc.robot.subsystems.drive.DriveSubsystem;
@@ -155,7 +157,13 @@ public void execute() {
155157
desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
156158
driveSubsystem.calculateController(desiredState);
157159

158-
if (shouldTransitionToServoing()) {
160+
double currX = driveSubsystem.getPoseMeters().getX();
161+
162+
if (shouldTransitionToServoing()
163+
&& (currX > DriveConstants.kCenterLineX
164+
&& robotStateSubsystem.getAllianceColor() == Alliance.Blue
165+
|| robotStateSubsystem.getAllianceColor() == Alliance.Red
166+
&& currX < DriveConstants.kCenterLineX)) {
159167
isServoing = true;
160168

161169
visionSubsystem.setIsAuto(true);

‎src/main/java/frc/robot/commands/auton/NonProcessorSlowMicCommand.java‎

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
package frc.robot.commands.auton;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.math.geometry.Rotation2d;
5+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
6+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
7+
import frc.robot.commands.drive.PrepOdomForAutoCommand;
8+
import frc.robot.commands.elevator.ZeroElevatorCommand;
9+
import frc.robot.commands.pathHandler.StartPathHandlerCommand;
10+
import frc.robot.constants.PathHandlerConstants;
11+
import frc.robot.subsystems.algae.AlgaeSubsystem;
12+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
13+
import frc.robot.subsystems.coral.CoralSubsystem;
14+
import frc.robot.subsystems.drive.DriveSubsystem;
15+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
16+
import frc.robot.subsystems.pathHandler.PathHandler;
17+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
18+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
19+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
20+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
21+
import frc.robot.subsystems.vision.VisionSubsystem;
22+
import java.util.ArrayList;
23+
import java.util.List;
24+
25+
public class NonProcessorSlowMicCommand extends SequentialCommandGroup
26+
implements AutoCommandInterface {
27+
private PathHandler pathHandler;
28+
private DriveSubsystem driveSubsystem;
29+
private DriveAutonServoCommand startPath;
30+
private CoralSubsystem coralSubsystem;
31+
private RobotStateSubsystem robotStateSubsystem;
32+
private VisionSubsystem visionSubsystem;
33+
private List<Character> NodeNames;
34+
private List<ScoringLevel> NodeLevels;
35+
private List<Double> lightDistances = new ArrayList<>();
36+
private List<Double> postOffsets = new ArrayList<>();
37+
private char startNode;
38+
39+
public NonProcessorSlowMicCommand(
40+
DriveSubsystem driveSubsystem,
41+
PathHandler pathHandler,
42+
RobotStateSubsystem robotStateSubsystem,
43+
AlgaeSubsystem algaeSubsystem,
44+
BiscuitSubsystem biscuitSubsystem,
45+
CoralSubsystem coralSubsystem,
46+
ElevatorSubsystem elevatorSubsystem,
47+
TagAlignSubsystem tagAlignSubsystem,
48+
VisionSubsystem visionSubsystem,
49+
String startPathName,
50+
List<Character> NodeNames,
51+
List<ScoringLevel> NodeLevels,
52+
List<Double> lightDistances,
53+
List<Double> postOffsets,
54+
char startNode,
55+
Boolean startScoreLeft,
56+
Pose2d startPose) {
57+
addRequirements(
58+
driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
59+
this.pathHandler = pathHandler;
60+
this.driveSubsystem = driveSubsystem;
61+
this.coralSubsystem = coralSubsystem;
62+
this.robotStateSubsystem = robotStateSubsystem;
63+
this.visionSubsystem = visionSubsystem;
64+
65+
this.NodeNames = NodeNames;
66+
this.NodeLevels = NodeLevels;
67+
this.startNode = startNode;
68+
this.lightDistances = lightDistances;
69+
this.postOffsets = postOffsets;
70+
71+
startPath =
72+
new DriveAutonServoCommand(
73+
driveSubsystem,
74+
tagAlignSubsystem,
75+
elevatorSubsystem,
76+
biscuitSubsystem,
77+
robotStateSubsystem,
78+
startPathName,
79+
true,
80+
true,
81+
false,
82+
startScoreLeft,
83+
postOffsets.get(0));
84+
postOffsets.remove(0);
85+
86+
addCommands(
87+
new SequentialCommandGroup(
88+
new ParallelCommandGroup(
89+
new PrepOdomForAutoCommand(
90+
robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
91+
new ZeroElevatorCommand(elevatorSubsystem)),
92+
// new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
93+
startPath,
94+
new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
95+
96+
// new ParallelCommandGroup(
97+
// new ZeroElevatorCommand(elevatorSubsystem),
98+
// new ResetOdometryCommand(
99+
// driveSubsystem, new Pose2d(7.1008875, 5.0756788, new Rotation2d(180))),
100+
// new SequentialCommandGroup(
101+
// new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
102+
// new ResetOdometryCommand(driveSubsystem, AutonConstants.kNonProcessorShallow))),
103+
// new ParallelCommandGroup(
104+
// // new WaitCommand(0.03), new SetVisionUpdatesCommand(visionSubsystem, true)),
105+
// startPath,
106+
// new WaitForButtonPressCommand(button),
107+
new StartPathHandlerCommand(
108+
pathHandler,
109+
PathHandlerConstants.kSlowShallowPathNames,
110+
NodeNames,
111+
NodeLevels,
112+
startNode,
113+
false)));
114+
}
115+
116+
@Override
117+
public void reassignAlliance() {
118+
startPath.reassignAlliance();
119+
driveSubsystem.teleResetGyro();
120+
coralSubsystem.setAutoPreload();
121+
robotStateSubsystem.setIsAutoPlacing(false);
122+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
123+
robotStateSubsystem.setGetAlgaeOnCycle(false);
124+
// robotStateSubsystem.setIsAuto(true);
125+
robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
126+
visionSubsystem.setVisionUpdating(true);
127+
pathHandler.setPathNames(PathHandlerConstants.kSlowShallowPathNames);
128+
pathHandler.setNodeNames(NodeNames);
129+
pathHandler.setNodeLevels(NodeLevels);
130+
pathHandler.setStartNode(startNode);
131+
pathHandler.setGetAlgaeLast(false);
132+
pathHandler.setLightDistances(lightDistances);
133+
pathHandler.setPostOffsets(postOffsets);
134+
// pathHandler.reassignAlliance();
135+
}
136+
}

‎src/main/java/frc/robot/commands/auton/StealOneAlgeaNoSuperCycleAutonCommand.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ public StealOneAlgeaNoSuperCycleAutonCommand(
9696
robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
9797
// new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
9898
firstPath,
99+
new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
99100
new WaitForElevBelowBarge(elevatorSubsystem),
100101
secondPath,
101102
thirdPath,

‎src/main/java/frc/robot/commands/auton/StealTwoAlgeaNoSuperCycleAutonCommand.java‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
package frc.robot.commands.auton;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.math.geometry.Rotation2d;
5+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
6+
import frc.robot.commands.drive.PrepOdomForAutoCommand;
7+
import frc.robot.commands.robotState.AutoScoreAlgaeCommand;
8+
import frc.robot.subsystems.algae.AlgaeSubsystem;
9+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
10+
import frc.robot.subsystems.coral.CoralSubsystem;
11+
import frc.robot.subsystems.drive.DriveSubsystem;
12+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
13+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
14+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
15+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
16+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
17+
import frc.robot.subsystems.vision.VisionSubsystem;
18+
19+
public class StealTwoAlgeaNoSuperCycleAutonCommand extends SequentialCommandGroup
20+
implements AutoCommandInterface {
21+
22+
private DriveSubsystem driveSubsystem;
23+
private DriveAutonServoCommand firstPath;
24+
private DriveAlgaeWaitAutonServoCommand secondPath;
25+
private DriveBargeAutonCommand thirdPath;
26+
private DriveAlgaeWaitAutonServoCommand fourthPath;
27+
private DriveBargeAutonCommand fifthPath;
28+
private CoralSubsystem coralSubsystem;
29+
private RobotStateSubsystem robotStateSubsystem;
30+
private VisionSubsystem visionSubsystem;
31+
32+
public StealTwoAlgeaNoSuperCycleAutonCommand(
33+
DriveSubsystem driveSubsystem,
34+
RobotStateSubsystem robotStateSubsystem,
35+
AlgaeSubsystem algaeSubsystem,
36+
BiscuitSubsystem biscuitSubsystem,
37+
CoralSubsystem coralSubsystem,
38+
ElevatorSubsystem elevatorSubsystem,
39+
TagAlignSubsystem tagAlignSubsystem,
40+
VisionSubsystem visionSubsystem,
41+
String firstPathName,
42+
String secondPathName,
43+
String thirdPathName,
44+
String fourthPathName,
45+
String fifthPathName,
46+
ScoringLevel OppAlgeaHeight1,
47+
ScoringLevel OppAlgeaHeight2,
48+
Pose2d startPose) {
49+
addRequirements(
50+
driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
51+
this.driveSubsystem = driveSubsystem;
52+
this.coralSubsystem = coralSubsystem;
53+
this.robotStateSubsystem = robotStateSubsystem;
54+
this.visionSubsystem = visionSubsystem;
55+
56+
firstPath =
57+
new DriveAutonServoCommand(
58+
driveSubsystem,
59+
tagAlignSubsystem,
60+
elevatorSubsystem,
61+
biscuitSubsystem,
62+
robotStateSubsystem,
63+
firstPathName,
64+
true,
65+
true,
66+
false,
67+
false,
68+
0.0);
69+
70+
secondPath =
71+
new DriveAlgaeWaitAutonServoCommand(
72+
driveSubsystem,
73+
tagAlignSubsystem,
74+
elevatorSubsystem,
75+
biscuitSubsystem,
76+
robotStateSubsystem,
77+
visionSubsystem,
78+
secondPathName,
79+
false,
80+
true,
81+
false,
82+
0.0,
83+
OppAlgeaHeight1,
84+
1.5);
85+
86+
thirdPath =
87+
new DriveBargeAutonCommand(
88+
driveSubsystem,
89+
tagAlignSubsystem,
90+
elevatorSubsystem,
91+
biscuitSubsystem,
92+
robotStateSubsystem,
93+
visionSubsystem,
94+
thirdPathName,
95+
true,
96+
false);
97+
98+
fourthPath =
99+
new DriveAlgaeWaitAutonServoCommand(
100+
driveSubsystem,
101+
tagAlignSubsystem,
102+
elevatorSubsystem,
103+
biscuitSubsystem,
104+
robotStateSubsystem,
105+
visionSubsystem,
106+
fourthPathName,
107+
false,
108+
true,
109+
false,
110+
0.0,
111+
OppAlgeaHeight2,
112+
0.0);
113+
114+
fifthPath =
115+
new DriveBargeAutonCommand(
116+
driveSubsystem,
117+
tagAlignSubsystem,
118+
elevatorSubsystem,
119+
biscuitSubsystem,
120+
robotStateSubsystem,
121+
visionSubsystem,
122+
fifthPathName,
123+
true,
124+
false);
125+
126+
addCommands(
127+
new SequentialCommandGroup(
128+
new PrepOdomForAutoCommand(
129+
robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
130+
// new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
131+
firstPath,
132+
new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
133+
new WaitForElevBelowBarge(elevatorSubsystem),
134+
secondPath,
135+
thirdPath,
136+
new AutoScoreAlgaeCommand(
137+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
138+
fourthPath,
139+
fifthPath,
140+
new AutoScoreAlgaeCommand(
141+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)));
142+
}
143+
144+
@Override
145+
public void reassignAlliance() {
146+
firstPath.reassignAlliance();
147+
secondPath.reassignAlliance();
148+
thirdPath.reassignAlliance();
149+
fourthPath.reassignAlliance();
150+
fifthPath.reassignAlliance();
151+
driveSubsystem.teleResetGyro();
152+
coralSubsystem.setAutoPreload();
153+
robotStateSubsystem.setIsAutoPlacing(false);
154+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
155+
robotStateSubsystem.setGetAlgaeOnCycle(false);
156+
// robotStateSubsystem.setIsAuto(true);
157+
robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
158+
visionSubsystem.setVisionUpdating(true);
159+
// pathHandler.reassignAlliance();
160+
}
161+
}

0 commit comments

Comments
 (0)