Skip to content

Commit c655367

Browse files
mwitcpalekmwitcpalek
committed
add auto versions with rear cams
1 parent 873ce25 commit c655367

File tree

5 files changed

+406
-19
lines changed

5 files changed

+406
-19
lines changed

‎README.md‎

Lines changed: 20 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -155,25 +155,26 @@
155155
| Rear Right | 10.27.67.13 | USB 2.0 |
156156

157157
## Autos
158-
| ID | Start Loc | Description | Status |
159-
| ---- | ------------------------ | -------------------------------- | ---------------- |
160-
| 0x00 | non-processor shallow | 3.5 piece Coral | Tested |
161-
| 0x01 | non-processor mid | 4 piece Coral | Needs Tuning |
162-
| 0x02 | non-processor mid | 3.5 piece Coral | Tested |
163-
| 0x03 | non-processor deep barge | IRI - 3 Algae | Partially Tested |
164-
| 0x04 | non-processor Kettering | 2 Mic to barge, Supercycle I | Tested |
165-
| 0x10 | Middle Barge | 2.5 Algae, Non-Processor First | Tested |
166-
| 0x11 | Middle Barge | 2.5 Algae, Processor First | Tested |
167-
| 0x12 | Middle Barge | 2 Algae, Non-Processor | Tested |
168-
| 0x13 | Middle Barge | 2 Algae, Processor | Tested |
169-
| 0x14 | Middle Barge | Super-Cycle then Steal Processor | Tested |
170-
| 0x15 | Middle Barge | Steal Immediately Processor | Tested |
171-
| 0x16 | Middle Barge | Coral then Steal Processor | Tested |
172-
| 0x17 | Middle Barge | Coral then Steal Two | Tested |
173-
| 0x18 | Middle Barge | Super-Cycle then Steal Front | Partially Tested |
174-
| 0x20 | processor shallow | 3.5 piece Coral | Tested |
175-
| 0x21 | processor shallow | 4 piece Coral | Needs Tuning |
176-
| 0x22 | processor mid | 3.5 piece Coral | Tested |
158+
| ID | Start Loc | Description | Status |
159+
| ---- | ------------------------ | ----------------------------------------- | ---------------- |
160+
| 0x00 | non-processor shallow | 3.5 piece Coral | Tested |
161+
| 0x01 | non-processor mid | 4 piece Coral | Needs Tuning |
162+
| 0x02 | non-processor mid | 3.5 piece Coral | Tested |
163+
| 0x03 | non-processor deep barge | IRI - 3 Algae | Partially Tested |
164+
| 0x04 | non-processor Kettering | 2 Mic to barge, Supercycle I | Tested |
165+
| 0x05 | non-processor Kettering | 2 Mic to barge, Supercycle I, rearCams on | Not Te |
166+
| 0x10 | Middle Barge | 2.5 Algae, Non-Processor First | Tested |
167+
| 0x11 | Middle Barge | 2.5 Algae, Processor First | Tested |
168+
| 0x12 | Middle Barge | 2 Algae, Non-Processor | Tested |
169+
| 0x13 | Middle Barge | 2 Algae, Processor | Tested |
170+
| 0x14 | Middle Barge | Super-Cycle then Steal Processor | Tested |
171+
| 0x15 | Middle Barge | Steal Immediately Processor | Tested |
172+
| 0x16 | Middle Barge | Coral then Steal Processor | Tested |
173+
| 0x17 | Middle Barge | Coral then Steal Two | Tested |
174+
| 0x18 | Middle Barge | Super-Cycle then Steal Front | Partially Tested |
175+
| 0x20 | processor shallow | 3.5 piece Coral | Tested |
176+
| 0x21 | processor shallow | 4 piece Coral | Needs Tuning |
177+
| 0x22 | processor mid | 3.5 piece Coral | Tested |
177178

178179
## LEDs
179180

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

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
{
2+
"name":"ketteringStartPathRearCams",
3+
"version":1,
4+
"snapshot":{
5+
"waypoints":[
6+
{"x":7.6, "y":7.01835, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
7+
{"x":8.146, "y":7.01835, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
8+
"constraints":[
9+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
10+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
11+
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
12+
"targetDt":0.05
13+
},
14+
"params":{
15+
"waypoints":[
16+
{"x":{"exp":"7.6 m", "val":7.6}, "y":{"exp":"7.01835 m", "val":7.01835}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
17+
{"x":{"exp":"8.146 m", "val":8.146}, "y":{"exp":"7.01835 m", "val":7.01835}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
18+
"constraints":[
19+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
20+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
21+
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
22+
"targetDt":{
23+
"exp":"0.05 s",
24+
"val":0.05
25+
}
26+
},
27+
"trajectory":{
28+
"sampleType":"Swerve",
29+
"waypoints":[0.0,0.43029],
30+
"samples":[
31+
{"t":0.0, "x":7.6, "y":7.01835, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.94973, "ay":0.0, "alpha":0.0, "fx":[199.1963,199.1963,199.1963,199.1963], "fy":[0.0,0.0,0.0,0.0]},
32+
{"t":0.04781, "x":7.61366, "y":7.01835, "heading":0.0, "vx":0.57132, "vy":0.0, "omega":0.0, "ax":11.94603, "ay":0.0, "alpha":0.0, "fx":[199.13463,199.13463,199.13463,199.13463], "fy":[0.0,0.0,0.0,0.0]},
33+
{"t":0.09562, "x":7.65463, "y":7.01835, "heading":0.0, "vx":1.14247, "vy":0.0, "omega":0.0, "ax":11.93864, "ay":0.0, "alpha":0.0, "fx":[199.01134,199.01134,199.01134,199.01134], "fy":[0.0,0.0,0.0,0.0]},
34+
{"t":0.14343, "x":7.72289, "y":7.01835, "heading":0.0, "vx":1.71326, "vy":0.0, "omega":0.0, "ax":11.91647, "ay":0.0, "alpha":0.0, "fx":[198.64189,198.64189,198.64189,198.64189], "fy":[0.0,0.0,0.0,0.0]},
35+
{"t":0.19124, "x":7.81842, "y":7.01835, "heading":0.0, "vx":2.28299, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
36+
{"t":0.23905, "x":7.92758, "y":7.01835, "heading":0.0, "vx":2.28299, "vy":0.0, "omega":0.0, "ax":-11.91647, "ay":0.0, "alpha":0.0, "fx":[-198.64189,-198.64189,-198.64189,-198.64189], "fy":[0.0,0.0,0.0,0.0]},
37+
{"t":0.28686, "x":8.02311, "y":7.01835, "heading":0.0, "vx":1.71326, "vy":0.0, "omega":0.0, "ax":-11.93864, "ay":0.0, "alpha":0.0, "fx":[-199.01134,-199.01134,-199.01134,-199.01134], "fy":[0.0,0.0,0.0,0.0]},
38+
{"t":0.33467, "x":8.09137, "y":7.01835, "heading":0.0, "vx":1.14247, "vy":0.0, "omega":0.0, "ax":-11.94603, "ay":0.0, "alpha":0.0, "fx":[-199.13463,-199.13463,-199.13463,-199.13463], "fy":[0.0,0.0,0.0,0.0]},
39+
{"t":0.38248, "x":8.13234, "y":7.01835, "heading":0.0, "vx":0.57132, "vy":0.0, "omega":0.0, "ax":-11.94973, "ay":0.0, "alpha":0.0, "fx":[-199.1963,-199.1963,-199.1963,-199.1963], "fy":[0.0,0.0,0.0,0.0]},
40+
{"t":0.43029, "x":8.146, "y":7.01835, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
41+
"splits":[0]
42+
},
43+
"events":[]
44+
}

‎src/main/java/frc/robot/commands/auton/KetteringAutoRearCamsCommand.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.DriveAutonRearCamCommand;
8+
import frc.robot.commands.drive.PrepOdomForAutoCommand;
9+
import frc.robot.commands.elevator.ZeroElevatorCommand;
10+
import frc.robot.commands.robotState.AutoForceBargeCommand;
11+
import frc.robot.commands.robotState.MicAlgaeCommand;
12+
import frc.robot.subsystems.algae.AlgaeSubsystem;
13+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
14+
import frc.robot.subsystems.coral.CoralSubsystem;
15+
import frc.robot.subsystems.drive.DriveSubsystem;
16+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
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.List;
23+
24+
public class KetteringAutoRearCamsCommand extends SequentialCommandGroup
25+
implements AutoCommandInterface {
26+
private DriveSubsystem driveSubsystem;
27+
private CoralSubsystem coralSubsystem;
28+
private RobotStateSubsystem robotStateSubsystem;
29+
private VisionSubsystem visionSubsystem;
30+
31+
private DriveAutonRearCamCommand bargeToAlgaePath;
32+
private DriveAlgaeAutonServoNoZeroCommand bargeToReefPath;
33+
private DriveAutonRearCamCommand reefToBargePath;
34+
private DriveAutonRearCamCommand bargeAwayPath;
35+
private DriveAutonRearCamCommand startPath;
36+
37+
public KetteringAutoRearCamsCommand(
38+
DriveSubsystem driveSubsystem,
39+
RobotStateSubsystem robotStateSubsystem,
40+
AlgaeSubsystem algaeSubsystem,
41+
BiscuitSubsystem biscuitSubsystem,
42+
CoralSubsystem coralSubsystem,
43+
ElevatorSubsystem elevatorSubsystem,
44+
TagAlignSubsystem tagAlignSubsystem,
45+
VisionSubsystem visionSubsystem,
46+
String startString,
47+
String bargeToAlgae,
48+
String bargeToReef,
49+
String reefToBarge,
50+
String bargeAway,
51+
List<RobotStateSubsystem.ScoringLevel> algaeLevels,
52+
Pose2d startPose) {
53+
addRequirements(
54+
driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
55+
this.driveSubsystem = driveSubsystem;
56+
this.coralSubsystem = coralSubsystem;
57+
this.robotStateSubsystem = robotStateSubsystem;
58+
this.visionSubsystem = visionSubsystem;
59+
60+
bargeToAlgaePath =
61+
new DriveAutonRearCamCommand(
62+
driveSubsystem, visionSubsystem, bargeToAlgae, true, false, false);
63+
bargeToReefPath =
64+
new DriveAlgaeAutonServoNoZeroCommand(
65+
driveSubsystem,
66+
tagAlignSubsystem,
67+
elevatorSubsystem,
68+
biscuitSubsystem,
69+
robotStateSubsystem,
70+
visionSubsystem,
71+
bargeToReef,
72+
true,
73+
true,
74+
false,
75+
0,
76+
algaeLevels.get(0));
77+
reefToBargePath =
78+
new DriveAutonRearCamCommand(
79+
driveSubsystem, visionSubsystem, reefToBarge, true, false, false);
80+
bargeAwayPath =
81+
new DriveAutonRearCamCommand(
82+
driveSubsystem, visionSubsystem, bargeAway, true, false, false);
83+
startPath =
84+
new DriveAutonRearCamCommand(
85+
driveSubsystem, visionSubsystem, startString, true, true, false);
86+
87+
addCommands(
88+
new ParallelCommandGroup(
89+
new PrepOdomForAutoCommand(
90+
robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(0.0), startPose),
91+
new ZeroElevatorCommand(elevatorSubsystem)),
92+
new ParallelCommandGroup(
93+
startPath,
94+
new MicAlgaeCommand(
95+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)),
96+
new AutoForceBargeCommand(
97+
robotStateSubsystem,
98+
elevatorSubsystem,
99+
biscuitSubsystem,
100+
algaeSubsystem,
101+
visionSubsystem),
102+
new WaitForElevBelowBarge(elevatorSubsystem),
103+
new ParallelCommandGroup(
104+
bargeToAlgaePath,
105+
new MicAlgaeCommand(
106+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)),
107+
new AutoForceBargeCommand(
108+
robotStateSubsystem,
109+
elevatorSubsystem,
110+
biscuitSubsystem,
111+
algaeSubsystem,
112+
visionSubsystem),
113+
bargeToReefPath,
114+
reefToBargePath);
115+
// new AutoScoreAlgaeCommand(
116+
// robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
117+
// bargeAwayPath);
118+
}
119+
120+
@Override
121+
public void reassignAlliance() {
122+
driveSubsystem.teleResetGyro();
123+
coralSubsystem.setAutoPreload();
124+
robotStateSubsystem.setIsAutoPlacing(true);
125+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
126+
robotStateSubsystem.setGetAlgaeOnCycle(true);
127+
robotStateSubsystem.setScoreSide(ScoreSide.LEFT);
128+
visionSubsystem.setVisionUpdating(true);
129+
130+
bargeToAlgaePath.reassignAlliance();
131+
bargeToReefPath.reassignAlliance();
132+
reefToBargePath.reassignAlliance();
133+
bargeAwayPath.reassignAlliance();
134+
startPath.reassignAlliance();
135+
}
136+
}

0 commit comments

Comments
 (0)