Skip to content

Commit e46d777

Browse files
EpicPigGuyEpicPigGuy
committed
update
1 parent f89f23c commit e46d777

File tree

2 files changed

+163
-0
lines changed

2 files changed

+163
-0
lines changed

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

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

‎src/main/java/frc/robot/subsystems/auto/AutoSwitch.java‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
99
import frc.robot.commands.auton.AutoCommandInterface;
1010
import frc.robot.commands.auton.DefaultAutonCommand;
11+
import frc.robot.commands.auton.NonProcessorMediumAutonCommand;
1112
import frc.robot.commands.auton.NonProcessorShallowAutonCommand;
1213
import frc.robot.commands.auton.NonProcessorShallowSlowAutonCommand;
1314
import frc.robot.commands.auton.ProcessorShallowAutonCommand;
@@ -283,6 +284,31 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
283284
AutonConstants.kNonProcessorMid);
284285
}
285286

287+
case 0x03 -> {
288+
return new NonProcessorShallowSlowAutonCommand(
289+
driveSubsystem,
290+
pathHandler,
291+
robotStateSubsystem,
292+
algaeSubsystem,
293+
biscuitSubsystem,
294+
coralSubsystem,
295+
elevatorSubsystem,
296+
tagAlignSubsystem,
297+
visionSubsystem,
298+
"startToJSlow",
299+
new ArrayList<Character>(Arrays.asList('k', 'l')),
300+
new ArrayList<ScoringLevel>(Arrays.asList(ScoringLevel.L4, ScoringLevel.L4)),
301+
new ArrayList<>(
302+
Arrays.asList(
303+
PathHandlerConstants.kLoadLightDistance,
304+
PathHandlerConstants.kLoadLightDistance,
305+
PathHandlerConstants.kLoadLightDistance)),
306+
new ArrayList<>(Arrays.asList(-0.0175, 0.0, 0.0)),
307+
'j',
308+
true,
309+
AutonConstants.kNonProcessorMid);
310+
}
311+
286312
case 0x20 -> {
287313
return new ProcessorShallowSlowAutonCommand(
288314
driveSubsystem,

0 commit comments

Comments
 (0)