Skip to content

Commit 80ac1e0

Browse files
ds12ads12a
committed
camera pitch and yaw, stealing autons, start path tweak
1 parent c3c16ad commit 80ac1e0

File tree

8 files changed

+834
-217
lines changed

8 files changed

+834
-217
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/midStartPtoESlow.traj‎

Lines changed: 75 additions & 78 deletions
Large diffs are not rendered by default.

‎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/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+
}

‎src/main/java/frc/robot/constants/VisionConstants.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -96,14 +96,16 @@ public final class VisionConstants {
9696
public static final int kCircularBufferSize = 1000;
9797
// Poses
9898
public static final Pose3d kCam1Pose =
99-
new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d());
99+
new Pose3d(new Translation3d(0.305, 0.0255, 0.311), new Rotation3d());
100100
public static final Pose3d kCam2Pose =
101101
new Pose3d(
102102
new Translation3d(-0.275, -0.21, 0.49),
103103
new Rotation3d(
104104
Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(145)));
105105
public static final Pose3d kCam3Pose =
106-
new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d());
106+
new Pose3d(
107+
new Translation3d(0.133, -0.3, 0.311), // -0.305
108+
new Rotation3d(0, Units.degreesToRadians(-2), Units.degreesToRadians(1)));
107109
public static final Pose3d kCam4Pose =
108110
new Pose3d(
109111
new Translation3d(-0.275, -0.145, 0.49),

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

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import frc.robot.commands.auton.StealAlgaeImmediately;
1717
import frc.robot.commands.auton.StealOneAlgeaAutonCommand;
1818
import frc.robot.commands.auton.StealOneAlgeaNoSuperCycleAutonCommand;
19+
import frc.robot.commands.auton.StealTwoAlgeaNoSuperCycleAutonCommand;
1920
import frc.robot.constants.AutonConstants;
2021
import frc.robot.constants.PathHandlerConstants;
2122
import frc.robot.constants.RobotConstants;
@@ -458,6 +459,26 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
458459
new Pose2d(7.1008875, 4.0509, Rotation2d.fromDegrees(180.0)));
459460
}
460461

462+
case 0x17 -> {
463+
return new StealTwoAlgeaNoSuperCycleAutonCommand(
464+
driveSubsystem,
465+
robotStateSubsystem,
466+
algaeSubsystem,
467+
biscuitSubsystem,
468+
coralSubsystem,
469+
elevatorSubsystem,
470+
tagAlignSubsystem,
471+
visionSubsystem,
472+
"startHToH",
473+
"HToOppE",
474+
"OppEToOppbarge",
475+
"OppBargeToOppG",
476+
"OppGToOppBarge",
477+
ScoringLevel.L3,
478+
ScoringLevel.L2,
479+
new Pose2d(7.1008875, 4.0509, Rotation2d.fromDegrees(180.0)));
480+
}
481+
461482
case 0x20 -> {
462483
return new ProcessorShallowSlowAutonCommand(
463484
driveSubsystem,

0 commit comments

Comments
 (0)