Skip to content

Commit 3eca5e5

Browse files
spellingcatspellingcat
authored
Merge pull request #88 from HighlanderRobotics/pre-dcmp
Pre dcmp
2 parents 3981928 + f39b252 commit 3eca5e5

10 files changed

Lines changed: 630 additions & 409 deletions

File tree

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

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

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

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

‎src/main/java/frc/robot/Autos.java‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,9 @@ public enum Path {
111111
RPreTrenchtoRNeutral("RPreTrenchtoRNeutral", Action.INTAKE),
112112
RPreTrenchReversedtoRNeutral("RPreTrenchReversedtoRNeutral", Action.INTAKE),
113113

114+
RPreTrenchReversedtoRCleanup("RPreTrenchReversedtoRCleanup", Action.INTAKE),
115+
EndWScoreRCleanuptoRPreTrenchReversed("RCleanuptoRPreTrenchReversed", Action.SCORE_AT_END),
116+
114117
StartingRTrenchtoRNeutral("StartingRTrenchtoRNeutral", Action.INTAKE),
115118
StartingLTrenchtoLNeutral("StartingLTrenchtoLNeutral", Action.INTAKE),
116119
StartingRTrenchtoDisrupt("StartingRTrenchtoDisruptR", Action.INTAKE),
@@ -559,8 +562,10 @@ public Command getDoubleDipRightTrench() {
559562
new Path[] {
560563
Path.StartingRTrenchtoRNeutral,
561564
Path.RNeutraltoRPreTrenchReversed,
562-
Path.RPreTrenchReversedtoRNeutral,
563-
Path.RNeutraltoRPreTrenchReversed,
565+
Path.RPreTrenchReversedtoRCleanup,
566+
Path.EndWScoreRCleanuptoRPreTrenchReversed
567+
// Path.RPreTrenchReversedtoRNeutral,
568+
// Path.RNeutraltoRPreTrenchReversed,
564569
},
565570
setRightClimb());
566571
}

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 139 additions & 176 deletions
Large diffs are not rendered by default.

‎src/main/java/frc/robot/Superstructure.java‎

Lines changed: 75 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -171,9 +171,7 @@ public boolean isScoringActive() {
171171

172172
@AutoLogOutput(key = "Superstructure/Score Request")
173173
private Trigger scoreReq =
174-
new Trigger(() -> shotTarget == ShotTarget.SCORE)
175-
// .and(() -> canScore())
176-
.or(Autos.autoScoreReq);
174+
new Trigger(() -> shotTarget == ShotTarget.SCORE).or(Autos.autoScoreReq);
177175

178176
@AutoLogOutput(key = "Superstructure/Feed Request")
179177
private Trigger feedReq = new Trigger(() -> shotTarget == ShotTarget.FEED);
@@ -193,9 +191,6 @@ public boolean isScoringActive() {
193191
@AutoLogOutput(key = "Superstructure/Ready?")
194192
private Trigger readyTrigger;
195193

196-
@AutoLogOutput(key = "Superstructure/Operator Pose Override?")
197-
private static boolean poseOverride = false;
198-
199194
@AutoLogOutput(key = "Superstructure/Defense?")
200195
private boolean defense = false;
201196

@@ -290,13 +285,8 @@ private void addTriggers() {
290285
operator.povUp().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
291286
operator.povDown().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
292287
shootReq =
293-
driver
294-
.rightTrigger()
295-
.and(DriverStation::isTeleop)
296-
.and(() -> canShoot())
297-
.or(Autos.autoScoreReq)
298-
.and(() -> canShoot()); // Maybe should include if its our turn? //TODO fix auto
299-
// bindings
288+
new Trigger(() -> awayFromTrench())
289+
.and((driver.rightTrigger().and(DriverStation::isTeleop)).or(Autos.autoScoreReq));
300290

301291
intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq);
302292

@@ -337,9 +327,7 @@ private void addTransitions() {
337327
// SCORE_FLOW transitions
338328
{
339329
bindTransition(
340-
SuperState.IDLE,
341-
SuperState.SPIN_UP_SCORE_FLOW,
342-
scoreReq.and(flowReq).and(shootReq.or(intakeReq)));
330+
SuperState.IDLE, SuperState.SPIN_UP_SCORE_FLOW, scoreReq.and(flowReq).and(shootReq));
343331

344332
bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger);
345333

@@ -348,17 +336,13 @@ private void addTransitions() {
348336
SuperState.SPIN_UP_SCORE_FLOW,
349337
new Trigger(AutoAim::targetInTurretDeadzone));
350338

351-
bindTransition(
352-
SuperState.SPIN_UP_SCORE_FLOW,
353-
SuperState.IDLE,
354-
intakeReq.negate().and(shootReq.negate()));
339+
bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, shootReq.negate());
355340

356341
bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq);
357342

358343
bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate());
359344

360-
bindTransition(
361-
SuperState.SCORE_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
345+
bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, shootReq.negate());
362346
}
363347

364348
// --------------------------------------------------------------------------
@@ -377,9 +361,7 @@ private void addTransitions() {
377361
// FEED_FLOW transitions
378362
{
379363
bindTransition(
380-
SuperState.IDLE,
381-
SuperState.SPIN_UP_FEED_FLOW,
382-
feedReq.and(flowReq).and(shootReq.or(intakeReq)));
364+
SuperState.IDLE, SuperState.SPIN_UP_FEED_FLOW, feedReq.and(flowReq).and(shootReq));
383365

384366
bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger);
385367

@@ -388,15 +370,13 @@ private void addTransitions() {
388370
SuperState.SPIN_UP_FEED_FLOW,
389371
new Trigger(AutoAim::targetInTurretDeadzone));
390372

391-
bindTransition(
392-
SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
373+
bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, shootReq.negate());
393374

394375
bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq);
395376

396377
bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate());
397378

398-
bindTransition(
399-
SuperState.FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
379+
bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, shootReq.negate());
400380
}
401381

402382
bindTransition(SuperState.SCORE, SuperState.FEED, feedReq);
@@ -487,13 +467,16 @@ private void addCommands() {
487467
SuperState.SPIN_UP_FEED,
488468
intake.restExtended(),
489469
indexer.rest(),
490-
shooter.feed(
491-
() ->
492-
AutoAim.getShotParameters(
493-
swerve.getPose(),
494-
swerve.getVelocityRobotRelative(),
495-
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
496-
ShotTrees.FEED_SHOT_TREE)),
470+
shooter
471+
.resetTurretToCalculatedPosition()
472+
.andThen(
473+
shooter.feed(
474+
() ->
475+
AutoAim.getShotParameters(
476+
swerve.getPose(),
477+
swerve.getVelocityRobotRelative(),
478+
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
479+
ShotTrees.FEED_SHOT_TREE))),
497480
climber.retract());
498481

499482
bindCommands(
@@ -521,13 +504,16 @@ private void addCommands() {
521504
SuperState.SPIN_UP_FEED_FLOW,
522505
intake.intake(),
523506
indexer.index(),
524-
shooter.feed(
525-
() ->
526-
AutoAim.getShotParameters(
527-
swerve.getPose(),
528-
swerve.getVelocityRobotRelative(),
529-
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
530-
ShotTrees.FEED_SHOT_TREE)),
507+
shooter
508+
.resetTurretToCalculatedPosition()
509+
.andThen(
510+
shooter.feed(
511+
() ->
512+
AutoAim.getShotParameters(
513+
swerve.getPose(),
514+
swerve.getVelocityRobotRelative(),
515+
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
516+
ShotTrees.FEED_SHOT_TREE))),
531517
climber.retract());
532518

533519
bindCommands(
@@ -587,18 +573,15 @@ private void addCommands() {
587573
.flywheelVelocityRotPerSec()
588574
// shooter.getTestVel()
589575
),
590-
shooter
591-
.resetTurretToCalculatedPosition()
592-
.andThen(
593-
shooter.score(
594-
() ->
595-
AutoAim.getShotParameters(
596-
swerve.getPose(),
597-
swerve.getVelocityRobotRelative(),
598-
FieldUtils.getCurrentHubTranslation(),
599-
Robot.ROBOT_EDITION == RobotEdition.ALPHA
600-
? ShotTrees.ALPHA_HUB_SHOT_TREE
601-
: ShotTrees.COMP_HUB_SHOT_TREE))),
576+
shooter.score(
577+
() ->
578+
AutoAim.getShotParameters(
579+
swerve.getPose(),
580+
swerve.getVelocityRobotRelative(),
581+
FieldUtils.getCurrentHubTranslation(),
582+
Robot.ROBOT_EDITION == RobotEdition.ALPHA
583+
? ShotTrees.ALPHA_HUB_SHOT_TREE
584+
: ShotTrees.COMP_HUB_SHOT_TREE)),
602585
// shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())),
603586
climber.retract());
604587

@@ -634,18 +617,15 @@ private void addCommands() {
634617
: ShotTrees.COMP_HUB_SHOT_TREE)
635618
.shotData()
636619
.flywheelVelocityRotPerSec()),
637-
shooter
638-
.resetTurretToCalculatedPosition()
639-
.andThen(
640-
shooter.score(
641-
() ->
642-
AutoAim.getShotParameters(
643-
swerve.getPose(),
644-
swerve.getVelocityRobotRelative(),
645-
FieldUtils.getCurrentHubTranslation(),
646-
Robot.ROBOT_EDITION == RobotEdition.ALPHA
647-
? ShotTrees.ALPHA_HUB_SHOT_TREE
648-
: ShotTrees.COMP_HUB_SHOT_TREE))),
620+
shooter.score(
621+
() ->
622+
AutoAim.getShotParameters(
623+
swerve.getPose(),
624+
swerve.getVelocityRobotRelative(),
625+
FieldUtils.getCurrentHubTranslation(),
626+
Robot.ROBOT_EDITION == RobotEdition.ALPHA
627+
? ShotTrees.ALPHA_HUB_SHOT_TREE
628+
: ShotTrees.COMP_HUB_SHOT_TREE)),
649629
climber.retract());
650630

651631
bindCommands(
@@ -685,18 +665,15 @@ private void addCommands() {
685665
SuperState.SPIN_UP_SCORE_PRE_CLIMB,
686666
intake.restRetracted(),
687667
indexer.rest(),
688-
shooter
689-
.resetTurretToCalculatedPosition()
690-
.andThen(
691-
shooter.score(
692-
() ->
693-
AutoAim.getShotParameters(
694-
swerve.getPose(),
695-
swerve.getVelocityRobotRelative(),
696-
FieldUtils.getCurrentHubTranslation(),
697-
Robot.ROBOT_EDITION == RobotEdition.ALPHA
698-
? ShotTrees.ALPHA_HUB_SHOT_TREE
699-
: ShotTrees.COMP_HUB_SHOT_TREE))),
668+
shooter.score(
669+
() ->
670+
AutoAim.getShotParameters(
671+
swerve.getPose(),
672+
swerve.getVelocityRobotRelative(),
673+
FieldUtils.getCurrentHubTranslation(),
674+
Robot.ROBOT_EDITION == RobotEdition.ALPHA
675+
? ShotTrees.ALPHA_HUB_SHOT_TREE
676+
: ShotTrees.COMP_HUB_SHOT_TREE)),
700677
climber.extend());
701678

702679
bindCommands(
@@ -713,18 +690,15 @@ private void addCommands() {
713690
: ShotTrees.COMP_HUB_SHOT_TREE)
714691
.shotData()
715692
.flywheelVelocityRotPerSec()),
716-
shooter
717-
.resetTurretToCalculatedPosition()
718-
.andThen(
719-
shooter.score(
720-
() ->
721-
AutoAim.getShotParameters(
722-
swerve.getPose(),
723-
swerve.getVelocityRobotRelative(),
724-
FieldUtils.getCurrentHubTranslation(),
725-
Robot.ROBOT_EDITION == RobotEdition.ALPHA
726-
? ShotTrees.ALPHA_HUB_SHOT_TREE
727-
: ShotTrees.COMP_HUB_SHOT_TREE))),
693+
shooter.score(
694+
() ->
695+
AutoAim.getShotParameters(
696+
swerve.getPose(),
697+
swerve.getVelocityRobotRelative(),
698+
FieldUtils.getCurrentHubTranslation(),
699+
Robot.ROBOT_EDITION == RobotEdition.ALPHA
700+
? ShotTrees.ALPHA_HUB_SHOT_TREE
701+
: ShotTrees.COMP_HUB_SHOT_TREE)),
728702
climber.extend());
729703

730704
bindCommands(
@@ -740,14 +714,14 @@ public void periodic() {
740714
Logger.recordOutput("Superstructure/State Timer", stateTimer.get());
741715

742716
// this really should be in robot.java but i cooked myself with the robot selecting thing
743-
Logger.recordOutput(
744-
"shooter sotm viz",
745-
new Pose3d(swerve.getPose())
746-
.transformBy(
747-
new Transform3d(
748-
new Translation3d(0, 0, 0.5),
749-
new Rotation3d(
750-
0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0))));
717+
// Logger.recordOutput(
718+
// "shooter sotm viz",
719+
// new Pose3d(swerve.getPose())
720+
// .transformBy(
721+
// new Transform3d(
722+
// new Translation3d(0, 0, 0.5),
723+
// new Rotation3d(
724+
// 0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0))));
751725
}
752726

753727
/**
@@ -918,16 +892,10 @@ public boolean inScoringArea() {
918892
}
919893

920894
public boolean canScore() {
921-
return
922-
// (isOurShift() || !DriverStation.isFMSAttached())
923-
// &&
924-
(inScoringArea() || poseOverride)
925-
&& (!swerve.isNearTrench() || poseOverride
926-
// || fixedShotTarget != FixedShotTarget.NONE
927-
);
895+
return inScoringArea() && !swerve.isNearTrench();
928896
}
929897

930-
public boolean canShoot() {
898+
public boolean awayFromTrench() {
931899
return !swerve.isNearTrenchForHood();
932900
}
933901

@@ -938,12 +906,4 @@ public static ShotTarget getShotTarget() {
938906
public static FeedTarget getFeedTarget() {
939907
return feedTarget;
940908
}
941-
942-
// public static FixedShotTarget getFixedShotTarget() {
943-
// return fixedShotTarget;
944-
// }
945-
946-
public static boolean getPoseOverride() {
947-
return poseOverride;
948-
}
949909
}

‎src/main/java/frc/robot/components/camera/Camera.java‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -106,10 +106,10 @@ public Optional<EstimatedRobotPose> update(PhotonPipelineResult result) {
106106
if (result.getTargets().size() < 1) {
107107
return Optional.empty();
108108
}
109-
if (Robot.ROBOT_MODE != RobotMode.REAL)
110-
Logger.recordOutput(
111-
"Vision/" + io.getName() + "/Best Distance",
112-
result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm());
109+
// if (Robot.ROBOT_MODE != RobotMode.REAL)
110+
// Logger.recordOutput(
111+
// "Vision/" + io.getName() + "/Best Distance",
112+
// result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm());
113113
Optional<EstimatedRobotPose> estPose = estimator.update(result);
114114
return estPose;
115115
}
@@ -163,12 +163,12 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {
163163
setSimPose(estPose, !inputs.stale);
164164

165165
// if (Robot.ROBOT_TYPE != RobotType.REAL)
166-
Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose);
166+
// Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose);
167167
// if (Robot.ROBOT_TYPE != RobotType.REAL)
168-
Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d());
168+
// Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d());
169169
final Matrix<N3, N1> deviations = findVisionMeasurementStdDevs(estPose.get());
170170
// if (Robot.ROBOT_TYPE != RobotType.REAL)
171-
Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData());
171+
// Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData());
172172

173173
Tracer.trace(
174174
"Add Measurement From " + getName(),
@@ -182,7 +182,7 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {
182182

183183
hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime();
184184
// if (Robot.ROBOT_TYPE != RobotType.REAL)
185-
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update");
185+
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update");
186186

187187
Tracer.trace(
188188
"Log Tag Poses",
@@ -196,16 +196,16 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {
196196
.get();
197197
}
198198
// if (Robot.ROBOT_TYPE != RobotType.REAL)
199-
Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds);
199+
// Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds);
200200
});
201201

202202
} else {
203203
// if (Robot.ROBOT_TYPE != RobotType.REAL)
204-
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale");
204+
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale");
205205
}
206206
} catch (NoSuchElementException e) {
207207
// if (Robot.ROBOT_TYPE != RobotType.REAL)
208-
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate");
208+
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate");
209209
}
210210
futureVisionData.set(hasFutureData);
211211
}

0 commit comments

Comments
 (0)