Skip to content

Commit 78c3c0e

Browse files
Superbro525AltSuperbro525Alt
committed
refactor
1 parent 5f7705f commit 78c3c0e

11 files changed

Lines changed: 2445 additions & 2144 deletions

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Setpoints.java‎

Lines changed: 33 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,13 @@
3939
import java.util.concurrent.atomic.AtomicBoolean;
4040
import java.util.concurrent.atomic.AtomicLong;
4141
import java.util.concurrent.atomic.AtomicReference;
42+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.Constraints;
4243
import org.curtinfrc.frc2026.util.Repulsor.Shooting.DragShotPlanner;
44+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.GamePiecePhysics;
45+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.OnlineSearchState;
46+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotLibrary;
47+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotLibraryBuilder;
48+
import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotSolution;
4349

4450
public class Setpoints {
4551

@@ -363,13 +369,13 @@ private Rebuilt2026() {}
363369
private static final double HUB_SIMPLE_RADIUS_M = 3.0;
364370
private static final double HUB_OBS_ENABLE_DIST_M = 2.15;
365371

366-
private static final DragShotPlanner.GamePiecePhysics HUB_PHYSICS = loadHubPhysicsOnce();
372+
private static final GamePiecePhysics HUB_PHYSICS = loadHubPhysicsOnce();
367373

368-
private static DragShotPlanner.GamePiecePhysics loadHubPhysicsOnce() {
374+
private static GamePiecePhysics loadHubPhysicsOnce() {
369375
try {
370376
return DragShotPlanner.loadGamePieceFromDeployYaml(GAME_PIECE_ID_FUEL);
371377
} catch (Throwable t) {
372-
return new DragShotPlanner.GamePiecePhysics() {
378+
return new GamePiecePhysics() {
373379
@Override
374380
public double massKg() {
375381
return 0.27;
@@ -401,7 +407,7 @@ private static final class HubLastPoseCache {
401407
volatile int lastSolveObsHash;
402408

403409
volatile long lastRefineNs;
404-
volatile DragShotPlanner.ShotSolution lastSolution;
410+
volatile ShotSolution lastSolution;
405411
}
406412

407413
private static final double[][] SIMPLE_OFFS =
@@ -523,8 +529,8 @@ private static Pose2d findFastValidPose(
523529
return safeValidFallback(targetFieldPos, halfL, halfW, obs);
524530
}
525531

526-
public static final DragShotPlanner.Constraints HUB_SHOT_CONSTRAINTS =
527-
new DragShotPlanner.Constraints(0, 30, 0, 45.0, DragShotPlanner.Constraints.ShotStyle.ARC);
532+
public static final Constraints HUB_SHOT_CONSTRAINTS =
533+
new Constraints(0, 30, 0, 45.0, Constraints.ShotStyle.ARC);
528534

529535
public static final int BLUE_HUB_ANCHOR_TAG_ID = 20;
530536
public static final int BLUE_OUTPOST_ANCHOR_TAG_ID = 13;
@@ -556,7 +562,7 @@ public static Translation2d hubAimpointForAlliance(Alliance alliance) {
556562
return alliance == Alliance.Red ? flipToRed(blue) : blue;
557563
}
558564

559-
public static Optional<DragShotPlanner.ShotSolution> getHubShotSolution(SetpointContext ctx) {
565+
public static Optional<ShotSolution> getHubShotSolution(SetpointContext ctx) {
560566
if (ctx == null || ctx.robotPose().isEmpty()) {
561567
return Optional.empty();
562568
}
@@ -586,8 +592,8 @@ private record HubShotCacheKey(
586592

587593
private static final ConcurrentHashMap<HubShotCacheKey, HubShotLibraryState> HUB_LIB_CACHE =
588594
new ConcurrentHashMap<>();
589-
private static final ConcurrentHashMap<HubShotCacheKey, DragShotPlanner.OnlineSearchState>
590-
HUB_ONLINE_STATE = new ConcurrentHashMap<>();
595+
private static final ConcurrentHashMap<HubShotCacheKey, OnlineSearchState> HUB_ONLINE_STATE =
596+
new ConcurrentHashMap<>();
591597
private static final ConcurrentHashMap<HubShotCacheKey, HubLastPoseCache> HUB_LAST_POSE =
592598
new ConcurrentHashMap<>();
593599

@@ -598,20 +604,20 @@ private record HubShotCacheKey(
598604

599605
private static final class HubShotLibraryState {
600606
final HubShotCacheKey key;
601-
final DragShotPlanner.GamePiecePhysics physics;
607+
final GamePiecePhysics physics;
602608
final Translation2d targetFieldPos;
603609
final double releaseH;
604610
final double halfL;
605611
final double halfW;
606612

607-
volatile DragShotPlanner.ShotLibrary published;
613+
volatile ShotLibrary published;
608614
volatile boolean done;
609615
final Object lock = new Object();
610-
DragShotPlanner.ShotLibraryBuilder builder;
616+
ShotLibraryBuilder builder;
611617

612618
HubShotLibraryState(
613619
HubShotCacheKey key,
614-
DragShotPlanner.GamePiecePhysics physics,
620+
GamePiecePhysics physics,
615621
Translation2d targetFieldPos,
616622
double releaseH,
617623
double halfL,
@@ -679,7 +685,7 @@ private static void hubBackgroundTick() {
679685
double bearingStep = 12.0;
680686

681687
st.builder =
682-
new DragShotPlanner.ShotLibraryBuilder(
688+
new ShotLibraryBuilder(
683689
st.physics,
684690
st.targetFieldPos,
685691
HUB_OPENING_FRONT_EDGE_HEIGHT_M,
@@ -692,7 +698,7 @@ private static void hubBackgroundTick() {
692698
radialStep,
693699
bearingStep);
694700
}
695-
DragShotPlanner.ShotLibrary publish = st.builder.maybeStep(900_000L);
701+
ShotLibrary publish = st.builder.maybeStep(900_000L);
696702
if (publish != null) {
697703
if (!publish.entries().isEmpty() || publish.complete()) {
698704
st.published = publish;
@@ -706,8 +712,8 @@ private static void hubBackgroundTick() {
706712
}
707713
}
708714

709-
private static DragShotPlanner.ShotLibrary getOrStartHubLibrary(
710-
DragShotPlanner.GamePiecePhysics physics,
715+
private static ShotLibrary getOrStartHubLibrary(
716+
GamePiecePhysics physics,
711717
Translation2d targetFieldPos,
712718
double releaseH,
713719
double halfL,
@@ -728,15 +734,14 @@ private static DragShotPlanner.ShotLibrary getOrStartHubLibrary(
728734
key,
729735
k -> new HubShotLibraryState(k, physics, targetFieldPos, releaseH, halfL, halfW));
730736

731-
DragShotPlanner.ShotLibrary pub = st.published;
737+
ShotLibrary pub = st.published;
732738
if (pub != null) return pub;
733739
return null;
734740
}
735741

736-
private static DragShotPlanner.OnlineSearchState getOrCreateOnlineState(
742+
private static OnlineSearchState getOrCreateOnlineState(
737743
HubShotCacheKey key, Translation2d seed) {
738-
return HUB_ONLINE_STATE.computeIfAbsent(
739-
key, k -> new DragShotPlanner.OnlineSearchState(seed, 0.58));
744+
return HUB_ONLINE_STATE.computeIfAbsent(key, k -> new OnlineSearchState(seed, 0.58));
740745
}
741746

742747
private static HubShotCacheKey hubKey(
@@ -902,12 +907,11 @@ private static Pose2d computeShootPose(SetpointContext ctx, Translation2d target
902907

903908
Translation2d seed = simple.getTranslation();
904909

905-
DragShotPlanner.ShotLibrary lib =
906-
getOrStartHubLibrary(HUB_PHYSICS, targetFieldPos, releaseH, halfL, halfW);
910+
ShotLibrary lib = getOrStartHubLibrary(HUB_PHYSICS, targetFieldPos, releaseH, halfL, halfW);
907911

908-
DragShotPlanner.ShotSolution libSol = null;
912+
ShotSolution libSol = null;
909913
if (lib != null && !lib.entries().isEmpty()) {
910-
Optional<DragShotPlanner.ShotSolution> opt =
914+
Optional<ShotSolution> opt =
911915
DragShotPlanner.findBestShotFromLibrary(
912916
lib,
913917
HUB_PHYSICS,
@@ -925,7 +929,7 @@ private static Pose2d computeShootPose(SetpointContext ctx, Translation2d target
925929
}
926930
}
927931

928-
DragShotPlanner.OnlineSearchState online = getOrCreateOnlineState(key, seed);
932+
OnlineSearchState online = getOrCreateOnlineState(key, seed);
929933

930934
int mdx = Math.abs(rxmm - state.lastSolveRobotXmm);
931935
int mdy = Math.abs(rymm - state.lastSolveRobotYmm);
@@ -950,10 +954,10 @@ private static Pose2d computeShootPose(SetpointContext ctx, Translation2d target
950954

951955
boolean allowRefine = !haveRecentSol && sinceRefine >= 120_000_000L;
952956

953-
DragShotPlanner.ShotSolution refinedSol = null;
957+
ShotSolution refinedSol = null;
954958
if (allowRefine) {
955959
long refineBudgetNs = useObs ? 160_000L : 120_000L;
956-
Optional<DragShotPlanner.ShotSolution> refined =
960+
Optional<ShotSolution> refined =
957961
DragShotPlanner.findBestShotOnlineRefine(
958962
HUB_PHYSICS,
959963
targetFieldPos,
@@ -974,7 +978,7 @@ private static Pose2d computeShootPose(SetpointContext ctx, Translation2d target
974978
}
975979
}
976980

977-
DragShotPlanner.ShotSolution sol = refinedSol != null ? refinedSol : libSol;
981+
ShotSolution sol = refinedSol != null ? refinedSol : libSol;
978982

979983
Pose2d out;
980984
if (sol == null) {

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/Constraints.java‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
/*
2+
* Copyright (C) 2026 Paul Hodges
3+
*
4+
* This file is part of Repulsor.
5+
*
6+
* Repulsor is free software: you can redistribute it and/or modify
7+
* it under the terms of the GNU General Public License as published by
8+
* the Free Software Foundation, either version 3 of the License, or
9+
* (at your option) any later version.
10+
*
11+
* Repulsor is distributed in the hope that it will be useful,
12+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
13+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14+
* GNU General Public License for more details.
15+
*
16+
* You should have received a copy of the GNU General Public License
17+
* along with Repulsor. If not, see https://www.gnu.org/licenses/.
18+
*/
19+
20+
package org.curtinfrc.frc2026.util.Repulsor.Shooting;
21+
22+
public final class Constraints {
23+
24+
public enum ShotStyle {
25+
ANY,
26+
DIRECT,
27+
ARC
28+
}
29+
30+
private final double minLaunchSpeedMetersPerSecond;
31+
private final double maxLaunchSpeedMetersPerSecond;
32+
private final double minLaunchAngleDeg;
33+
private final double maxLaunchAngleDeg;
34+
private final ShotStyle shotStyle;
35+
36+
public Constraints(
37+
double minLaunchSpeedMetersPerSecond,
38+
double maxLaunchSpeedMetersPerSecond,
39+
double minLaunchAngleDeg,
40+
double maxLaunchAngleDeg) {
41+
this(
42+
minLaunchSpeedMetersPerSecond,
43+
maxLaunchSpeedMetersPerSecond,
44+
minLaunchAngleDeg,
45+
maxLaunchAngleDeg,
46+
ShotStyle.ANY);
47+
}
48+
49+
public Constraints(
50+
double minLaunchSpeedMetersPerSecond,
51+
double maxLaunchSpeedMetersPerSecond,
52+
double minLaunchAngleDeg,
53+
double maxLaunchAngleDeg,
54+
ShotStyle shotStyle) {
55+
this.minLaunchSpeedMetersPerSecond = minLaunchSpeedMetersPerSecond;
56+
this.maxLaunchSpeedMetersPerSecond = maxLaunchSpeedMetersPerSecond;
57+
this.minLaunchAngleDeg = minLaunchAngleDeg;
58+
this.maxLaunchAngleDeg = maxLaunchAngleDeg;
59+
this.shotStyle = shotStyle == null ? ShotStyle.ANY : shotStyle;
60+
}
61+
62+
public double minLaunchSpeedMetersPerSecond() {
63+
return minLaunchSpeedMetersPerSecond;
64+
}
65+
66+
public double maxLaunchSpeedMetersPerSecond() {
67+
return maxLaunchSpeedMetersPerSecond;
68+
}
69+
70+
public double minLaunchAngleDeg() {
71+
return minLaunchAngleDeg;
72+
}
73+
74+
public double maxLaunchAngleDeg() {
75+
return maxLaunchAngleDeg;
76+
}
77+
78+
public ShotStyle shotStyle() {
79+
return shotStyle;
80+
}
81+
}

0 commit comments

Comments
 (0)