Skip to content

Commit 56f8c42

Browse files
Superbro525AltSuperbro525Alt
committed
improvements
1 parent 5bda6a9 commit 56f8c42

File tree

9 files changed

+173
-48
lines changed

9 files changed

+173
-48
lines changed

‎calibration_board.pdf‎

52 KB
Binary file not shown.

‎calibration_board.png‎

122 KB
Loading

‎repulsor_3d_sim/repulsor_3d_sim/model.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,3 +50,5 @@ class WorldSnapshot:
5050
truth: List[FieldVisionObject]
5151
pose: Optional[Pose2d]
5252
extrinsics: Tuple[float, float, float, float, float, float]
53+
active_goal: Optional[Pose2d]
54+
chosen_collect: Optional[Pose2d]

‎repulsor_3d_sim/repulsor_3d_sim/nt4.py‎

Lines changed: 28 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,13 @@ def __init__(self, cfg: ViewerConfig):
9797
pose_topic = self.inst.getStructTopic(_join_topic(self._pose_prefix, cfg.pose_struct_key), Pose2d)
9898
self._pose_struct = pose_topic.subscribe(Pose2d(0.0, 0.0, Rotation2d()))
9999

100+
101+
active_pose_topic = self.inst.getStructTopic("/AdvantageKit/RealOutputs/ActiveGoal", Pose2d);
102+
self.active_pose = active_pose_topic.subscribe(Pose2d(0.0, 0.0, Rotation2d()))
103+
104+
collect_pose_topic = self.inst.getStructTopic("/AdvantageKit/RealOutputs/Repulsor/ChosenCollect", Pose2d);
105+
self.collect_pose = collect_pose_topic.subscribe(Pose2d(0.0, 0.0, Rotation2d()))
106+
100107
self._ex = self.inst.getDoubleTopic(_join_topic(self._fv_prefix, "extrinsics/x")).subscribe(0.0)
101108
self._ey = self.inst.getDoubleTopic(_join_topic(self._fv_prefix, "extrinsics/y")).subscribe(0.0)
102109
self._ez = self.inst.getDoubleTopic(_join_topic(self._fv_prefix, "extrinsics/z")).subscribe(0.0)
@@ -214,6 +221,24 @@ def read_pose2d(self) -> Optional[Pose2d]:
214221
return None
215222
return Pose2d(x, y, Rotation2d.fromDegrees(th_deg))
216223

224+
def read_pose2d_active(self) -> Optional[Pose2d]:
225+
p: Pose2d = self.active_pose.get()
226+
x = float(p.x)
227+
y = float(p.y)
228+
th_deg = float(p.rotation().degrees())
229+
if x != x or y != y or th_deg != th_deg:
230+
return None
231+
return Pose2d(x, y, Rotation2d.fromDegrees(th_deg))
232+
233+
def read_pose2d_chosen_collect(self) -> Optional[Pose2d]:
234+
p: Pose2d = self.collect_pose.get()
235+
x = float(p.x)
236+
y = float(p.y)
237+
th_deg = float(p.rotation().degrees())
238+
if x != x or y != y or th_deg != th_deg:
239+
return None
240+
return Pose2d(x, y, Rotation2d.fromDegrees(th_deg))
241+
217242
def read_extrinsics(self) -> Tuple[float, float, float, float, float, float]:
218243
return (
219244
float(self._ex.get()),
@@ -291,4 +316,6 @@ def snapshot(self) -> WorldSnapshot:
291316
truth: List[FieldVisionObject] = []
292317
pose = self.read_pose2d()
293318
ex = self.read_extrinsics()
294-
return WorldSnapshot(fv, rv, cams, truth, pose, ex)
319+
active_goal = self.read_pose2d_active()
320+
chosen_collect = self.read_pose2d_chosen_collect()
321+
return WorldSnapshot(fv, rv, cams, truth, pose, ex, active_goal, chosen_collect)

‎repulsor_3d_sim/repulsor_3d_sim/render/scene.py‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -116,6 +116,8 @@ def __init__(self, cfg: ViewerConfig):
116116
self._col_truth_fuel = (0.15, 0.95, 0.35, 0.70)
117117
self._col_other = (1.0, 0.15, 0.15, 0.85)
118118
self._col_us = (1, 0.27, 0, 0.75)
119+
self._col_active = (0.15, 0.9, 0.3, 0.9)
120+
self._col_chosen_collect = (0.15, 0.3, 0.9, 0.9)
119121
self._col_heading = (1.0, 0.1, 0.1, 0.9)
120122
self._col_cam = (0.20, 0.75, 1.0, 0.85)
121123
self._col_cam_fov = (0.20, 0.75, 1.0, 0.55)
@@ -406,6 +408,29 @@ def _draw_us_robot(self, snap: WorldSnapshot):
406408
draw_box((cx, cy, cz), (self._robot_l, self._robot_w, self._robot_h), yaw_deg, self._col_us)
407409
self._draw_robot_heading_arrow(cx, cy, yaw_deg)
408410

411+
412+
pose2 = snap.active_goal
413+
# print(pose2)
414+
if pose2 is None:
415+
return
416+
417+
cx = float(pose2.x)
418+
cy = float(pose2.y)
419+
cz = self._field_z + (self._robot_h * 0.5)
420+
yaw_deg = float(pose2.rotation().degrees())
421+
draw_box((cx, cy, cz), (self._robot_l, self._robot_w, self._robot_h), yaw_deg, self._col_active)
422+
# self._draw_robot_heading_arrow(cx, cy, yaw_deg)
423+
424+
pose3 = snap.chosen_collect
425+
if pose3 is None:
426+
return
427+
428+
cx = float(pose3.x)
429+
cy = float(pose3.y)
430+
cz = self._field_z + (self._robot_h * 0.5)
431+
yaw_deg = float(pose3.rotation().degrees())
432+
draw_box((cx, cy, cz), (self._robot_l, self._robot_w, self._robot_h), yaw_deg, self._col_chosen_collect)
433+
409434
def _draw_robot_heading_arrow(self, cx: float, cy: float, yaw_deg: float) -> None:
410435
yaw_rad = math.radians(float(yaw_deg))
411436
length = max(self._robot_l, self._robot_w) * 0.95

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/FieldPlanner/Helpers/FieldPlannerGoalManager.java‎

Lines changed: 66 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,9 @@
2424
import edu.wpi.first.math.geometry.Translation2d;
2525
import java.util.List;
2626
import org.curtinfrc.frc2026.util.Repulsor.Constants;
27-
import org.curtinfrc.frc2026.util.Repulsor.ExtraPathing;
2827
import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Obstacle;
2928
import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Obstacles.GatedAttractorObstacle;
29+
import org.littletonrobotics.junction.Logger;
3030

3131
public final class FieldPlannerGoalManager {
3232
private static final double STAGED_CENTER_BAND_M = 3.648981;
@@ -36,6 +36,7 @@ public final class FieldPlannerGoalManager {
3636

3737
private static final double STAGED_REACH_ENTER_M = 0.35;
3838
private static final double STAGED_REACH_EXIT_M = 0.55;
39+
private static final double STAGED_GATE_CLEAR_EXIT_M = 0.40;
3940
private static final int STAGED_REACH_TICKS = 3;
4041
private static final int STAGED_GATE_CLEAR_TICKS = 2;
4142

@@ -48,10 +49,14 @@ public final class FieldPlannerGoalManager {
4849

4950
private static final double STAGED_GATE_PAD_M = 0.25;
5051
private static final double STAGED_PASSED_X_HYST_M = 0.35;
52+
private static final double STAGED_PASSED_TARGET_PROJ_M = 0.16;
5153
private static final double STAGED_GOAL_SIDE_PROJ_M = 0.05;
54+
private static final double STAGED_LEAD_THROUGH_SCALE = 0.28;
55+
private static final double STAGED_LEAD_THROUGH_MIN_M = 0.45;
56+
private static final double STAGED_LEAD_THROUGH_MAX_M = 1.05;
5257
private static final double STAGED_DEEP_CENTER_BAND_M = 1.40;
53-
private static final double STAGED_CENTER_RETURN_STAGE_TRIGGER_M = 2.2;
54-
private static final double STAGED_CENTER_RETURN_INTERSECTION_TRIGGER_M = 3.0;
58+
private static final double STAGED_CENTER_RETURN_STAGE_TRIGGER_M = 3.0;
59+
private static final double STAGED_CENTER_RETURN_INTERSECTION_TRIGGER_M = 4.2;
5560
private static final double STAGED_CENTER_RETURN_EXIT_MIN_M = 0.70;
5661
private static final double STAGED_CENTER_RETURN_EXIT_MAX_M = 2.40;
5762
private static final double STAGED_CENTER_RETURN_GATE_MIN_OFFSET_M = 2.0;
@@ -105,6 +110,7 @@ public void setRequestedGoal(Pose2d requested) {
105110

106111
public void setActiveGoal(Pose2d active) {
107112
this.goal = active;
113+
Logger.recordOutput("ActiveGoal", this.goal);
108114
}
109115

110116
public boolean updateStagedGoal(Translation2d curPos, List<? extends Obstacle> obstacles) {
@@ -128,14 +134,14 @@ public boolean updateStagedGoal(Translation2d curPos, List<? extends Obstacle> o
128134
}
129135

130136
Translation2d reqT = requestedGoal.getTranslation();
131-
GatedAttractorObstacle firstBlock = firstOccludingGateAlongSegment(curPos, reqT, obstacles);
137+
GatedAttractorObstacle firstBlock = firstOccludingGateAlongSegment(curPos, reqT);
132138

133139
boolean stageForOccludingGate =
134140
firstBlock != null && !shouldDeferCenterReturnStage(curPos, reqT, firstBlock);
135141
boolean shouldStage = shouldStageThroughAttractor(curPos, reqT) || stageForOccludingGate;
136142
if (stagedComplete && lastStagedPoint != null) {
137143
double d = curPos.getDistance(lastStagedPoint);
138-
if (d < STAGED_RESTAGE_DIST_M) {
144+
if (d < STAGED_RESTAGE_DIST_M && !stageForOccludingGate) {
139145
shouldStage = false;
140146
} else {
141147
stagedComplete = false;
@@ -202,6 +208,8 @@ public boolean updateStagedGoal(Translation2d curPos, List<? extends Obstacle> o
202208
Translation2d liveTarget =
203209
(stagedGate != null) ? stagingPullPoint(stagedGate, curPos, reqT) : stagedAttractor;
204210
if (liveTarget == null) liveTarget = stagedAttractor;
211+
boolean passedLiveTargetTowardGoal =
212+
hasPassedPointTowardGoal(curPos, liveTarget, reqT, STAGED_PASSED_TARGET_PROJ_M);
205213

206214
double d = curPos.getDistance(liveTarget);
207215

@@ -219,19 +227,24 @@ public boolean updateStagedGoal(Translation2d curPos, List<? extends Obstacle> o
219227
stagedGatePassed
220228
|| gateIsBehind(curPos, reqT, stagedGate)
221229
|| gateOnGoalSide(curPos, reqT, stagedGate)
222-
|| !gateOccludingNow;
230+
|| !gateOccludingNow
231+
|| passedLiveTargetTowardGoal;
223232
}
224233

225234
boolean gateCleared = (stagedGate == null) || stagedGatePassed;
226235
if (gateCleared) stagedGateClearTicks++;
227236
else stagedGateClearTicks = 0;
228237

229-
if (!reached && stagedGate != null && stagedGateClearTicks >= STAGED_GATE_CLEAR_TICKS)
238+
if (!reached
239+
&& stagedGate != null
240+
&& stagedGateClearTicks >= STAGED_GATE_CLEAR_TICKS
241+
&& (passedLiveTargetTowardGoal || d <= STAGED_GATE_CLEAR_EXIT_M)) {
230242
reached = true;
231-
if (!reached && stagedGate != null && gateCleared && d <= STAGED_REACH_EXIT_M) reached = true;
243+
}
244+
if (!reached && stagedGate == null && passedLiveTargetTowardGoal) reached = true;
232245

233246
if (stagedModeTicks >= STAGED_MAX_TICKS) {
234-
GatedAttractorObstacle nowFirst = firstOccludingGateAlongSegment(curPos, reqT, obstacles);
247+
GatedAttractorObstacle nowFirst = firstOccludingGateAlongSegment(curPos, reqT);
235248
if (nowFirst == null || stagedGatePassed) {
236249
reached = true;
237250
gateCleared = true;
@@ -408,7 +421,7 @@ private static boolean gateOnGoalSide(
408421
}
409422

410423
private GatedAttractorObstacle firstOccludingGateAlongSegment(
411-
Translation2d pos, Translation2d target, List<? extends Obstacle> obstacles) {
424+
Translation2d pos, Translation2d target) {
412425
if (gatedAttractors.isEmpty() || pos == null || target == null) return null;
413426

414427
GatedAttractorObstacle best = null;
@@ -424,14 +437,6 @@ private GatedAttractorObstacle firstOccludingGateAlongSegment(
424437
Translation2d[] poly = expandPoly(gate.gatePoly, STAGED_GATE_PAD_M);
425438

426439
if (!FieldPlannerGeometry.segmentIntersectsPolygonOuter(pos, target, poly)) continue;
427-
if (!ExtraPathing.isClearPath(
428-
"WaypointByp",
429-
pos,
430-
target,
431-
obstacles,
432-
org.curtinfrc.frc2026.Constants.ROBOT_X,
433-
org.curtinfrc.frc2026.Constants.ROBOT_Y,
434-
false)) continue;
435440
double t = firstIntersectionT(pos, target, poly);
436441
if (t < bestT - tieEps) {
437442
bestT = t;
@@ -570,10 +575,53 @@ private Translation2d stagingPullPoint(
570575
: (insideOnRight ? outside : inside))
571576
: center;
572577

578+
pick = extendPullPointThroughGate(pick, center, target);
579+
573580
if (gate == stagedGate) stagedLatchedPull = pick;
574581
return pick;
575582
}
576583

584+
private static boolean hasPassedPointTowardGoal(
585+
Translation2d pos, Translation2d point, Translation2d goal, double projMeters) {
586+
if (pos == null || point == null || goal == null) return false;
587+
588+
Translation2d toGoal = goal.minus(point);
589+
double n = toGoal.getNorm();
590+
if (n <= 1e-6) return pos.getDistance(point) <= Math.max(0.0, projMeters);
591+
592+
Translation2d dir = toGoal.div(n);
593+
Translation2d toPos = pos.minus(point);
594+
double proj = toPos.getX() * dir.getX() + toPos.getY() * dir.getY();
595+
return proj >= Math.max(0.0, projMeters);
596+
}
597+
598+
private static Translation2d extendPullPointThroughGate(
599+
Translation2d pullPoint, Translation2d gateCenter, Translation2d target) {
600+
if (pullPoint == null || gateCenter == null || target == null) return pullPoint;
601+
602+
double dx = target.getX() - gateCenter.getX();
603+
double sign = Math.signum(dx);
604+
if (Math.abs(sign) < 1e-9) return pullPoint;
605+
606+
double lead =
607+
MathUtil.clamp(
608+
Math.abs(dx) * STAGED_LEAD_THROUGH_SCALE,
609+
STAGED_LEAD_THROUGH_MIN_M,
610+
STAGED_LEAD_THROUGH_MAX_M);
611+
612+
double x =
613+
MathUtil.clamp(
614+
pullPoint.getX() + sign * lead,
615+
STAGED_FIELD_EDGE_MARGIN_M,
616+
Constants.FIELD_LENGTH - STAGED_FIELD_EDGE_MARGIN_M);
617+
double y =
618+
MathUtil.clamp(
619+
pullPoint.getY(),
620+
STAGED_FIELD_EDGE_MARGIN_M,
621+
Constants.FIELD_WIDTH - STAGED_FIELD_EDGE_MARGIN_M);
622+
return new Translation2d(x, y);
623+
}
624+
577625
private GatedAttractorObstacle chooseBestGateByScore(
578626
Translation2d pos, Translation2d target, GatedAttractorObstacle preferredGate) {
579627

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Fields/Rebuilt2026.java‎

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -160,17 +160,17 @@ public List<Obstacle> fieldObstacles() {
160160
double gapTopY = Constants.FIELD_WIDTH - (gapHeight * 0.5);
161161
double gapBottomY = gapHeight * 0.5;
162162

163-
double rectStrength = 2.5;
164-
double rectRangeX = 1.2;
165-
double rectRangeY = 1.5;
163+
double rectStrength = 4.2;
164+
double rectRangeX = 1.55;
165+
double rectRangeY = 2.0;
166166

167167
double biasStrength = 0.3;
168168
double biasRange = 1.2;
169169

170170
double bypassStrengthScale = 1.2;
171171
double bypassRange = 1.4;
172172

173-
double edgeOffset = 0.06;
173+
double edgeOffset = 0.48;
174174

175175
double rectHalfX = rectWidth * 0.5;
176176
Translation2d[] leftGate =
@@ -273,31 +273,35 @@ public List<Obstacle> fieldObstacles() {
273273
leftGate,
274274
new Translation2d(leftInsideX, gapTopY),
275275
sideBypassStrengthScale,
276-
sideBypassRange),
276+
sideBypassRange,
277+
true),
277278
new GatedAttractorObstacle(
278279
new Translation2d(leftPullXOut, gapTopY),
279280
sideBiasStrength,
280281
sideBiasRange,
281282
leftGate,
282283
new Translation2d(leftInsideX, gapTopY),
283284
sideBypassStrengthScale,
284-
sideBypassRange),
285+
sideBypassRange,
286+
true),
285287
new GatedAttractorObstacle(
286288
new Translation2d(leftPullXIn, gapBottomY),
287289
sideBiasStrength,
288290
sideBiasRange,
289291
leftGate,
290292
new Translation2d(leftInsideX, gapBottomY),
291293
sideBypassStrengthScale,
292-
sideBypassRange),
294+
sideBypassRange,
295+
true),
293296
new GatedAttractorObstacle(
294297
new Translation2d(leftPullXOut, gapBottomY),
295298
sideBiasStrength,
296299
sideBiasRange,
297300
leftGate,
298301
new Translation2d(leftInsideX, gapBottomY),
299302
sideBypassStrengthScale,
300-
sideBypassRange),
303+
sideBypassRange,
304+
true),
301305
new GatedAttractorObstacle(
302306
new Translation2d(rightPullXIn, gapTopY),
303307
sideBiasStrength,

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -77,9 +77,9 @@ static boolean isShooterPoseValidInternal(
7777
return false;
7878
}
7979

80-
if (!(x < minBand && x > maxBand) && !checkBounds) {
81-
return false;
82-
}
80+
// if (!(x < minBand && x > maxBand) && !checkBounds) {
81+
// return false;
82+
// }
8383

8484
double dx = targetFieldPosition.getX() - x;
8585
double dy = targetFieldPosition.getY() - y;

0 commit comments

Comments
 (0)