Skip to content

Commit ba13690

Browse files
mwitcpalekmwitcpalek
committed
auto path tweaks
1 parent 7ae0a13 commit ba13690

File tree

11 files changed

+274
-116
lines changed

11 files changed

+274
-116
lines changed

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

Lines changed: 74 additions & 72 deletions
Large diffs are not rendered by default.

‎src/main/java/frc/robot/commands/drive/DriveAutonServoCommand.java‎ renamed to ‎src/main/java/frc/robot/commands/auton/DriveAutonServoCommand.java‎

Lines changed: 27 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.robot.commands.drive;
1+
package frc.robot.commands.auton;
22

33
import choreo.Choreo;
44
import choreo.trajectory.SwerveSample;
@@ -8,11 +8,15 @@
88
import edu.wpi.first.wpilibj.DriverStation.Alliance;
99
import edu.wpi.first.wpilibj.Timer;
1010
import edu.wpi.first.wpilibj2.command.Command;
11-
import frc.robot.commands.auton.AutoCommandInterface;
11+
import frc.robot.commands.drive.DriveAutonCommand;
1212
import frc.robot.constants.AutonConstants;
1313
import frc.robot.constants.DriveConstants;
1414
import frc.robot.constants.PathHandlerConstants;
15+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
1516
import frc.robot.subsystems.drive.DriveSubsystem;
17+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
18+
import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates;
19+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
1620
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
1721
import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates;
1822
import java.util.Optional;
@@ -22,6 +26,8 @@
2226
public class DriveAutonServoCommand extends Command implements AutoCommandInterface {
2327
private final DriveSubsystem driveSubsystem;
2428
private final TagAlignSubsystem tagAlignSubsystem;
29+
private final ElevatorSubsystem elevatorSubsystem;
30+
private final RobotStateSubsystem robotStateSubsystem;
2531

2632
private Trajectory<SwerveSample> trajectory;
2733
private final Timer timer = new Timer();
@@ -35,6 +41,7 @@ public class DriveAutonServoCommand extends Command implements AutoCommandInterf
3541
private boolean resetOdometry;
3642
private boolean lastPath;
3743
private boolean scoreLeft;
44+
private boolean hasStaged = false;
3845

3946
private SwerveSample desiredState;
4047
private Pose2d finalPose;
@@ -43,15 +50,20 @@ public class DriveAutonServoCommand extends Command implements AutoCommandInterf
4350
public DriveAutonServoCommand(
4451
DriveSubsystem driveSubsystem,
4552
TagAlignSubsystem tagAlignSubsystem,
53+
ElevatorSubsystem elevatorSubsystem,
54+
BiscuitSubsystem biscuitSubsystem,
55+
RobotStateSubsystem robotStateSubsystem,
4656
String trajectoryName,
4757
boolean lastPath,
4858
boolean resetOdometry,
4959
boolean mirrorToProcessor,
5060
boolean scoreLeft) {
5161

52-
addRequirements(driveSubsystem);
62+
addRequirements(driveSubsystem, elevatorSubsystem, biscuitSubsystem);
5363
this.driveSubsystem = driveSubsystem;
5464
this.tagAlignSubsystem = tagAlignSubsystem;
65+
this.elevatorSubsystem = elevatorSubsystem;
66+
this.robotStateSubsystem = robotStateSubsystem;
5567

5668
this.resetOdometry = resetOdometry;
5769
this.lastPath = lastPath;
@@ -133,7 +145,10 @@ public void initialize() {
133145
// driveSubsystem.resetOdometry(initialPose);
134146
// driveSubsystem.resetHolonomicController();
135147
// }
148+
elevatorSubsystem.zero();
136149
isServoing = false;
150+
hasStaged = false;
151+
137152
if (isTherePath) {
138153
driveSubsystem.setEnableHolo(true);
139154
// driveSubsystem.recordAutoTrajectory(trajectory);
@@ -152,13 +167,18 @@ public void initialize() {
152167

153168
@Override
154169
public void execute() {
170+
if (elevatorSubsystem.getState() == ElevatorStates.ZEROED && !hasStaged) {
171+
hasStaged = true;
172+
robotStateSubsystem.toAutonPrestage();
173+
}
155174
if (isTherePath) {
156175
if (!isServoing) {
157176
desiredState = mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get());
158177
driveSubsystem.calculateController(desiredState);
159178

160179
if (shouldTransitionToServoing()) {
161180
isServoing = true;
181+
robotStateSubsystem.toPrepCoral();
162182
tagAlignSubsystem.startAuto(
163183
mirrorTrajectory ? Alliance.Red : Alliance.Blue, scoreLeft, false);
164184
}
@@ -178,8 +198,9 @@ public boolean isFinished() {
178198
if (!isTherePath) {
179199
return true;
180200
}
181-
return (timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout)
182-
|| tagAlignSubsystem.getState() == TagAlignStates.DONE && isServoing);
201+
return ((timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout)
202+
|| tagAlignSubsystem.getState() == TagAlignStates.DONE && isServoing))
203+
&& elevatorSubsystem.isFinished();
183204
// || (FastMath.sqrt(
184205
// FastMath.pow(driveSubsystem.getPoseMeters().getX() - finalPose.getX(), 2)
185206
// + FastMath.pow(
@@ -202,6 +223,7 @@ public void end(boolean interrupted) {
202223
driveSubsystem.drive(0, 0, 0);
203224
}
204225
isServoing = false;
226+
tagAlignSubsystem.terminate();
205227

206228
driveSubsystem.grapherTrajectoryActive(false);
207229
logger.info("End Trajectory {}: {}", trajectoryName, timer.get());

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

Lines changed: 39 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,9 @@
11
package frc.robot.commands.auton;
22

33
import edu.wpi.first.math.geometry.Rotation2d;
4-
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
54
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
6-
import edu.wpi.first.wpilibj2.command.WaitCommand;
7-
import frc.robot.commands.drive.DriveAutonServoCommand;
8-
import frc.robot.commands.drive.ResetOdometryCommand;
95
import frc.robot.commands.drive.SetGyroOffsetCommand;
10-
import frc.robot.commands.elevator.ZeroElevatorCommand;
116
import frc.robot.commands.pathHandler.StartPathHandlerCommand;
12-
import frc.robot.commands.vision.SetVisionUpdatesCommand;
13-
import frc.robot.constants.AutonConstants;
147
import frc.robot.constants.PathHandlerConstants;
158
import frc.robot.subsystems.algae.AlgaeSubsystem;
169
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
@@ -19,6 +12,8 @@
1912
import frc.robot.subsystems.elevator.ElevatorSubsystem;
2013
import frc.robot.subsystems.pathHandler.PathHandler;
2114
import frc.robot.subsystems.robotState.RobotStateSubsystem;
15+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
16+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
2217
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
2318
import frc.robot.subsystems.vision.VisionSubsystem;
2419
import java.util.List;
@@ -30,6 +25,9 @@ public class NonProcessorShallowAutonCommand extends SequentialCommandGroup
3025
private PathHandler pathHandler;
3126
private DriveSubsystem driveSubsystem;
3227
private DriveAutonServoCommand startPath;
28+
private CoralSubsystem coralSubsystem;
29+
private RobotStateSubsystem robotStateSubsystem;
30+
private VisionSubsystem visionSubsystem;
3331

3432
public NonProcessorShallowAutonCommand(
3533
DriveSubsystem driveSubsystem,
@@ -50,24 +48,40 @@ public NonProcessorShallowAutonCommand(
5048
driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
5149
this.pathHandler = pathHandler;
5250
this.driveSubsystem = driveSubsystem;
51+
this.coralSubsystem = coralSubsystem;
52+
this.robotStateSubsystem = robotStateSubsystem;
53+
this.visionSubsystem = visionSubsystem;
5354

5455
startPath =
5556
new DriveAutonServoCommand(
56-
driveSubsystem, tagAlignSubsystem, startPathName, true, true, false, false);
57+
driveSubsystem,
58+
tagAlignSubsystem,
59+
elevatorSubsystem,
60+
biscuitSubsystem,
61+
robotStateSubsystem,
62+
startPathName,
63+
true,
64+
true,
65+
false,
66+
false);
5767

5868
addCommands(
5969
new SequentialCommandGroup(
60-
new ParallelCommandGroup(
61-
new ZeroElevatorCommand(elevatorSubsystem),
62-
// new ResetOdometryCommand(
63-
// driveSubsystem, new Pose2d(7.1008875, 5.0756788, new Rotation2d(180))),
64-
new SequentialCommandGroup(
65-
new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
66-
new ResetOdometryCommand(driveSubsystem, AutonConstants.kNonProcessorShallow))),
67-
new ParallelCommandGroup(
68-
new WaitCommand(0.03), new SetVisionUpdatesCommand(visionSubsystem, true)),
70+
new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
6971
startPath,
70-
new WaitForButtonPressCommand(button),
72+
new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
73+
74+
// new ParallelCommandGroup(
75+
// new ZeroElevatorCommand(elevatorSubsystem),
76+
// new ResetOdometryCommand(
77+
// driveSubsystem, new Pose2d(7.1008875, 5.0756788, new Rotation2d(180))),
78+
// new SequentialCommandGroup(
79+
// new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
80+
// new ResetOdometryCommand(driveSubsystem, AutonConstants.kNonProcessorShallow))),
81+
// new ParallelCommandGroup(
82+
// // new WaitCommand(0.03), new SetVisionUpdatesCommand(visionSubsystem, true)),
83+
// startPath,
84+
// new WaitForButtonPressCommand(button),
7185
new StartPathHandlerCommand(
7286
pathHandler,
7387
PathHandlerConstants.kShallowPathNames,
@@ -81,6 +95,13 @@ public NonProcessorShallowAutonCommand(
8195
public void reassignAlliance() {
8296
startPath.reassignAlliance();
8397
driveSubsystem.teleResetGyro();
98+
coralSubsystem.setAutoPreload();
99+
robotStateSubsystem.setIsAutoPlacing(false);
100+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
101+
robotStateSubsystem.setGetAlgaeOnCycle(false);
102+
robotStateSubsystem.setIsAuto(true);
103+
robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
104+
visionSubsystem.setVisionUpdating(true);
84105
// pathHandler.reassignAlliance();
85106
}
86107
}

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

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package frc.robot.commands.auton;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.coral.CoralSubsystem;
5+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
6+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
7+
8+
public class PlaceCoralAutonCommand extends Command {
9+
private RobotStateSubsystem robotStateSubsystem;
10+
private CoralSubsystem coralSubsystem;
11+
12+
public PlaceCoralAutonCommand(
13+
RobotStateSubsystem robotStateSubsystem, CoralSubsystem coralSubsystem) {
14+
this.robotStateSubsystem = robotStateSubsystem;
15+
this.coralSubsystem = coralSubsystem;
16+
addRequirements(coralSubsystem);
17+
}
18+
19+
@Override
20+
public void initialize() {
21+
robotStateSubsystem.toPrepCoral();
22+
}
23+
24+
@Override
25+
public boolean isFinished() {
26+
return robotStateSubsystem.getState() != RobotStates.PLACE_CORAL && !coralSubsystem.hasCoral();
27+
}
28+
}

‎src/main/java/frc/robot/commands/robotState/AutonSetPrestageCommand.java‎

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

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ public class TagServoingConstants {
6666

6767
public static final double kEndDriveCurrentThreshold = 25;
6868
public static final int kEndCountThreshold = 1;
69-
public static final double kEndVelThreshold = 2;
69+
public static final double kEndVelThreshold = 4;
7070

7171
// Reef
7272
public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259);

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -82,10 +82,20 @@ public void registerWith(TelemetryService telemetry) {
8282
@Override
8383
public void zero() {
8484
didZero = false;
85-
double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1);
86-
double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos);
85+
double pos = MathUtil.inputModulus(rawPulseWidth.refresh().getValueAsDouble(), 0, 1);
86+
double pos2 = MathUtil.inputModulus(rawPulseWidth.refresh().getValueAsDouble(), 0, 1);
87+
double pos3 = MathUtil.inputModulus(rawPulseWidth.refresh().getValueAsDouble(), 0, 1);
88+
double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos3);
8789
talon.setPosition(setPos);
88-
logger.info("set Biscuit position to " + setPos);
90+
logger.info(
91+
"set Biscuit position to "
92+
+ setPos
93+
+ ", Abs Pos 1: "
94+
+ pos
95+
+ ", Abs Pos 2: "
96+
+ pos2
97+
+ "Abs Pos 3: "
98+
+ pos3);
8999
didZero = true;
90100
}
91101
}

‎src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,11 +32,16 @@ public AngularVelocity getSpeed() {
3232
return inputs.velocity;
3333
}
3434

35-
public void setState(CoralState state) {
35+
private void setState(CoralState state) {
3636
logger.info("{} -> {}", curState, state);
3737
this.curState = state;
3838
}
3939

40+
public void setAutoPreload() {
41+
intake();
42+
setState(CoralState.HAS_CORAL);
43+
}
44+
4045
@Override
4146
public void setSpeed(AngularVelocity speed) {
4247
setpoint = speed;

‎src/main/java/frc/robot/subsystems/drive/Swerve.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -330,7 +330,9 @@ public void updateInputs(SwerveIOInputs inputs) {
330330
+ drive13StatorCurrent.getValueAsDouble())
331331
/ 4;
332332
inputs.avgRearDriveVel =
333-
(drive12Velocity.getValueAsDouble() + drive13Velocity.getValueAsDouble()) / 2;
333+
(Math.abs(drive12Velocity.getValueAsDouble())
334+
+ Math.abs(drive13Velocity.getValueAsDouble()))
335+
/ 2;
334336
}
335337

336338
@Override

0 commit comments

Comments
 (0)