Skip to content

Commit 720efe4

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #74 from strykeforce/3-piece-tune
3 piece Auto Variants
2 parents 27dd5b6 + cca5e09 commit 720efe4

File tree

7 files changed

+746
-0
lines changed

7 files changed

+746
-0
lines changed

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

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

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

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

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

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

‎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/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/commands/robotState/MicAlgaeCommand.java‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
7+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
8+
import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight;
9+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
10+
11+
public class MicAlgaeCommand extends Command {
12+
private RobotStateSubsystem robotState;
13+
private boolean hasTriedToPickup = false;
14+
15+
public MicAlgaeCommand(
16+
RobotStateSubsystem robotState,
17+
ElevatorSubsystem elevatorSubsystem,
18+
BiscuitSubsystem biscuitSubsystem,
19+
AlgaeSubsystem algaeSubsystem) {
20+
this.robotState = robotState;
21+
addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem);
22+
}
23+
24+
@Override
25+
public void initialize() {
26+
hasTriedToPickup = false;
27+
robotState.setAlgaeHeight(AlgaeHeight.HIGH);
28+
robotState.toAlgaeFloorPickup();
29+
}
30+
31+
@Override
32+
public boolean isFinished() {
33+
if (robotState.getState() == RobotStates.MIC_ALGAE) {
34+
hasTriedToPickup = true;
35+
}
36+
return robotState.getState() == RobotStates.STOW && hasTriedToPickup;
37+
}
38+
}

0 commit comments

Comments
 (0)