Skip to content

Commit dfc8974

Browse files
Huck-RichardsonHuck-Richardson
committed
Added a auto that scores two coral and then super cycles. It's not finished, todo list in the new auton.
1 parent e46d777 commit dfc8974

File tree

2 files changed

+164
-1
lines changed

2 files changed

+164
-1
lines changed

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

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
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 NonProcessorMediumCycleAutonCommand 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 NonProcessorMediumCycleAutonCommand(
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+
new StartPathHandlerCommand(
97+
pathHandler,
98+
PathHandlerConstants.kShallowPathNames,
99+
NodeNames,
100+
NodeLevels,
101+
startNode,
102+
false)),
103+
// TODO Make function to detect and score coral before we drive the last path
104+
// TODO Make sure it works
105+
// TODO Make one like this for the barge
106+
new DriveAutonServoCommand(
107+
driveSubsystem,
108+
tagAlignSubsystem,
109+
elevatorSubsystem,
110+
biscuitSubsystem,
111+
robotStateSubsystem,
112+
"KTofetchSlow.traj",
113+
true,
114+
false,
115+
false,
116+
true,
117+
0.0));
118+
}
119+
120+
@Override
121+
public void reassignAlliance() {
122+
startPath.reassignAlliance();
123+
driveSubsystem.teleResetGyro();
124+
coralSubsystem.setAutoPreload();
125+
robotStateSubsystem.setIsAutoPlacing(false);
126+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
127+
robotStateSubsystem.setGetAlgaeOnCycle(false);
128+
// robotStateSubsystem.setIsAuto(true);
129+
robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
130+
visionSubsystem.setVisionUpdating(true);
131+
pathHandler.setPathNames(PathHandlerConstants.kProcessorSlowShallowPathNames);
132+
pathHandler.setNodeNames(NodeNames);
133+
pathHandler.setNodeLevels(NodeLevels);
134+
pathHandler.setStartNode(startNode);
135+
pathHandler.setGetAlgaeLast(lastAlgae);
136+
pathHandler.setLightDistances(lightDistances);
137+
pathHandler.setPostOffsets(postOffsets);
138+
// pathHandler.reassignAlliance();
139+
}
140+
}

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

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +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;
11+
import frc.robot.commands.auton.NonProcessorMediumCycleAutonCommand;
1212
import frc.robot.commands.auton.NonProcessorShallowAutonCommand;
1313
import frc.robot.commands.auton.NonProcessorShallowSlowAutonCommand;
1414
import frc.robot.commands.auton.ProcessorShallowAutonCommand;
@@ -380,6 +380,29 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
380380
AutonConstants.kProcessorMid);
381381
}
382382

383+
case 0x04 -> {
384+
return new NonProcessorMediumCycleAutonCommand(
385+
driveSubsystem,
386+
pathHandler,
387+
robotStateSubsystem,
388+
algaeSubsystem,
389+
biscuitSubsystem,
390+
coralSubsystem,
391+
elevatorSubsystem,
392+
tagAlignSubsystem,
393+
visionSubsystem,
394+
"midStartToJSlow",
395+
new ArrayList<Character>(Arrays.asList('l', 'k')),
396+
new ArrayList<ScoringLevel>(
397+
Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L4)),
398+
new ArrayList<>(Arrays.asList(0.0, 0.0, 0.0)),
399+
new ArrayList<>(Arrays.asList(0.0, 0.0, 0.0)),
400+
'j',
401+
false,
402+
true,
403+
AutonConstants.kNonProcessorMid);
404+
}
405+
383406
default -> {
384407
String msg = String.format("no auto command assigned for switch pos: %02X", switchPos);
385408
DriverStation.reportWarning(msg, false);

0 commit comments

Comments
 (0)