Skip to content

Commit a2f67e1

Browse files
nlaverdurecaparomulaPez357
authored
Remember verified scoring poses (#143)
* Convert cameras to subsystems, add function to get vision-only pose estimate, use it in pose-seek. * Added some code to store and load override values for coral pipe poses. * Finished logic for auto-align target correction. * Split X/Y gain for DriveToPose * Fix NaN test * Use wpilib Preferences not Java's * Create config class for accessing named preferences * Add d-padd button to fix auto-align pose * Revert DriveToPose speeds logic * Revert PID constants for DriveToPoseCommand * Switch to field April tags * Added Aapril tag json, removed reef offset, updated climber servo constants * Remove reef offset todo * morning tweaks * Fix elevator rocket * Move zorro controller command into teleop init. * sycronized blue and red auto logic * removed withtimout on elevator * added timout to both red and blue L4autos * Set default stop command for swerve in autonomous * Prevent elevator rocket in joystick command * made the elevator and robot move at the same time. * Tweaked the choro by 3 inches * Widen tolerance for pose seek display * Remove init from lifter joystick command * Tweaked the redCenterToL4G choreo * commented out the createjoystickcontrolcommand and tweaked choreo. * Another attempt to prevent skyrocket on teleop init * Adjusted path for 1 piece auto * Add a few metrics to smart dashboard * Slowed down the red Auto in Choero * Adjust Al;gae L3 height, comment out algae wrist set angle on grab * Uncomment jostick elevator control * Added pure odometry tracking for comparison with vision * Calibrate odometry in robot mode init functions * Replace Robot.updateOdometry() with DriveTrain.calibrateOdometry() * Remove unnecessary write of scheduler to NT * Record outtake pose as potential override * Add back stem gym tag layout * Reduce max velocity in DriveToPoseCommand * A little refactoring * Publish camera pose estimates to network tables * spotless * Fix null pointer in Vision camera polling. Switch to field layout * updated climber retract setpoint * intake corals in parallel with CG, and stop intaking when hasCoral * increase voltage of outtake algae to barge * spotless --------- Co-authored-by: Christopher Larrieu <[email protected]> Co-authored-by: Pez357 <[email protected]>
1 parent a283a51 commit a2f67e1

File tree

15 files changed

+515
-117
lines changed

15 files changed

+515
-117
lines changed

‎src/main/java/frc/game/Reef.java

Lines changed: 89 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,9 @@
1010
import edu.wpi.first.math.geometry.Translation2d;
1111
import edu.wpi.first.units.measure.Distance;
1212
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
13+
import frc.lib.AutoAlignTarget;
14+
import frc.lib.Config;
15+
import java.util.Optional;
1316

1417
/**
1518
* Enumerates the Reefs on the field.
@@ -45,6 +48,10 @@ public static Reef getNearestReef(Pose2d atPose) {
4548
private Pose2d centerPose;
4649

4750
Reef(Pose2d centerPose) {
51+
var adjust = Config.getDefault().loadTransform("Reef." + this.name());
52+
if (adjust.isPresent()) {
53+
centerPose = centerPose.transformBy(adjust.get());
54+
}
4855
this.centerPose = centerPose;
4956
}
5057

@@ -74,10 +81,6 @@ public int getSector(Pose2d atPose) {
7481
var delta = centerPose.getTranslation().minus(atPose.getTranslation());
7582
int vectorAngle = (int) delta.getAngle().getDegrees();
7683
int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360;
77-
// SmartDashboard.putNumber("Sector delta X", delta.getX());
78-
// SmartDashboard.putNumber("Sector delta Y", delta.getY());
79-
// SmartDashboard.putNumber("Sector vector delta", vectorAngle);
80-
// SmartDashboard.putNumber("Sector ray angle", rayAngle);
8184
return rayAngle / 60;
8285
}
8386

@@ -117,6 +120,34 @@ public enum Face {
117120
private Pose2d centerPose;
118121
private Pose2d leftPipePose;
119122
private Pose2d rightPipePose;
123+
private Optional<Pose2d> memoLeftPipePose;
124+
private Optional<Pose2d> memoRightPipePose;
125+
126+
class PipeTarget extends AutoAlignTarget {
127+
private final boolean isLeft;
128+
129+
public PipeTarget(boolean isLeft) {
130+
this.isLeft = isLeft;
131+
}
132+
133+
@Override
134+
public Pose2d getPose() {
135+
return isLeft ? getLeftPipePose() : getRightPipePose();
136+
}
137+
138+
@Override
139+
public void memoize() {
140+
getNewPose()
141+
.ifPresent(
142+
pose -> {
143+
if (isLeft) {
144+
memoizeLeftPipePose(pose);
145+
} else {
146+
memoizeRightPipePose(pose);
147+
}
148+
});
149+
}
150+
}
120151

121152
Face(Reef reef, int index) {
122153
this.reef = reef;
@@ -133,7 +164,9 @@ public enum Face {
133164
new Transform2d(
134165
new Translation2d(Meters.of(0), pipeSpacing.div(-2)), Rotation2d.kZero));
135166

136-
SmartDashboard.putString("ReefFace." + this.toString(), centerPose.toString());
167+
memoLeftPipePose = Config.getDefault().loadPose2d("Reef.Face." + this.name() + ".leftPipe");
168+
memoRightPipePose = Config.getDefault().loadPose2d("Reef.Face." + this.name() + ".rightPipe");
169+
SmartDashboard.putString("Reef.Face." + this.name(), centerPose.toString());
137170
}
138171

139172
/**
@@ -154,13 +187,36 @@ public Pose2d getCenterPose() {
154187
return centerPose;
155188
}
156189

190+
/**
191+
* Return the left pipe as an auto-align target
192+
*
193+
* @return left pipe target
194+
*/
195+
public AutoAlignTarget getLeftPipe() {
196+
return new PipeTarget(true);
197+
}
198+
199+
/**
200+
* Return the right pipe as an auto-align target
201+
*
202+
* @return right pipe target
203+
*/
204+
public AutoAlignTarget getRightPipe() {
205+
return new PipeTarget(false);
206+
}
207+
157208
/**
158209
* Gets the left pipe pose of this face.
159210
*
160211
* @return the left pipe pose of this face
161212
*/
162213
public Pose2d getLeftPipePose() {
163-
return leftPipePose;
214+
if (memoLeftPipePose.isEmpty() && memoRightPipePose.isPresent()) {
215+
var transform =
216+
new Transform2d(new Translation2d(Meters.of(0), pipeSpacing), Rotation2d.kZero);
217+
memoLeftPipePose = Optional.of(memoRightPipePose.get().transformBy(transform));
218+
}
219+
return memoLeftPipePose.orElse(leftPipePose);
164220
}
165221

166222
/**
@@ -169,7 +225,33 @@ public Pose2d getLeftPipePose() {
169225
* @return the right pipe pose of this face
170226
*/
171227
public Pose2d getRightPipePose() {
172-
return rightPipePose;
228+
if (memoRightPipePose.isEmpty() && memoLeftPipePose.isPresent()) {
229+
var transform =
230+
new Transform2d(
231+
new Translation2d(Meters.of(0), pipeSpacing.unaryMinus()), Rotation2d.kZero);
232+
memoRightPipePose = Optional.of(memoLeftPipePose.get().transformBy(transform));
233+
}
234+
return memoRightPipePose.orElse(rightPipePose);
235+
}
236+
237+
/**
238+
* Store the specified pose as an override for the derived value for the left pipe. Temporarily
239+
*
240+
* @param pose the override pose
241+
*/
242+
public void memoizeLeftPipePose(Pose2d pose) {
243+
memoLeftPipePose = Optional.of(pose);
244+
Config.getDefault().storePose2d("Reef.Face." + this.toString() + ".leftPipe", pose);
245+
}
246+
247+
/**
248+
* Store the specified pose as an override for the derived value for the left pipe.
249+
*
250+
* @param pose the override pose
251+
*/
252+
public void memoizeRightPipePose(Pose2d pose) {
253+
memoRightPipePose = Optional.of(pose);
254+
Config.getDefault().storePose2d("Reef.Face." + this.toString() + ".rightPipe", pose);
173255
}
174256
}
175257
}

‎src/main/java/frc/lib/AutoAlignTarget.java

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
package frc.lib;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import java.util.Optional;
5+
6+
public abstract class AutoAlignTarget {
7+
private Pose2d newPose;
8+
9+
/** Return the pose associated with this target */
10+
public abstract Pose2d getPose();
11+
12+
/** Set the pose associated with this target */
13+
public void setPose(Pose2d pose) {
14+
newPose = pose;
15+
}
16+
17+
public Optional<Pose2d> getNewPose() {
18+
return Optional.ofNullable(newPose);
19+
}
20+
21+
/** Store the set pose as an override for future uses of the target pose. */
22+
public abstract void memoize();
23+
}

‎src/main/java/frc/lib/Config.java

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
package frc.lib;
2+
3+
import static edu.wpi.first.units.Units.Degrees;
4+
5+
import edu.wpi.first.math.geometry.Pose2d;
6+
import edu.wpi.first.math.geometry.Rotation2d;
7+
import edu.wpi.first.math.geometry.Transform2d;
8+
import edu.wpi.first.math.geometry.Translation2d;
9+
import edu.wpi.first.wpilibj.Preferences;
10+
import java.util.Optional;
11+
12+
public class Config {
13+
14+
private static Config defaultConfig =
15+
new Config(Preferences.getString("defaultConfig", "default"));
16+
17+
public static Config getDefault() {
18+
return defaultConfig;
19+
}
20+
21+
private final String name;
22+
23+
public Config(String name) {
24+
this.name = name;
25+
}
26+
27+
public String getName() {
28+
return name;
29+
}
30+
31+
public String prefix(String key) {
32+
return name + "." + key;
33+
}
34+
35+
public void storeDouble(String key, double value) {
36+
Preferences.setDouble(prefix(key), value);
37+
}
38+
39+
public Optional<Double> loadDouble(String key) {
40+
var value = Preferences.getDouble(prefix(key), Double.NaN);
41+
if (Double.isNaN(value)) {
42+
return Optional.empty();
43+
}
44+
return Optional.of(value);
45+
}
46+
47+
public void storeTranslation2d(String key, Translation2d xy) {
48+
storeDouble(key + ".x", xy.getX());
49+
storeDouble(key + ".y", xy.getY());
50+
}
51+
52+
public Optional<Translation2d> loadTranslation2d(String key) {
53+
var x = loadDouble(key + ".x");
54+
var y = loadDouble(key + ".y");
55+
if (x.isEmpty() || y.isEmpty()) {
56+
return Optional.empty();
57+
}
58+
return Optional.of(new Translation2d(x.get(), y.get()));
59+
}
60+
61+
public void storeRotation2d(String key, Rotation2d w) {
62+
storeDouble(key + ".w", w.getDegrees());
63+
}
64+
65+
public Optional<Rotation2d> loadRotation2d(String key) {
66+
var w = loadDouble(key + ".w");
67+
if (w.isEmpty()) {
68+
return Optional.empty();
69+
}
70+
return Optional.of(new Rotation2d(Degrees.of(w.get())));
71+
}
72+
73+
public void storeTransform(String key, Transform2d value) {
74+
storeTranslation2d(key, value.getTranslation());
75+
storeRotation2d(key, value.getRotation());
76+
}
77+
78+
public Optional<Transform2d> loadTransform(String key) {
79+
var translation = loadTranslation2d(key);
80+
var rotation = loadRotation2d(key);
81+
if (translation.isEmpty() || rotation.isEmpty()) {
82+
return Optional.empty();
83+
}
84+
return Optional.of(new Transform2d(translation.get(), rotation.get()));
85+
}
86+
87+
public void storePose2d(String key, Pose2d value) {
88+
storeTranslation2d(key, value.getTranslation());
89+
storeRotation2d(key, value.getRotation());
90+
}
91+
92+
public Optional<Pose2d> loadPose2d(String key) {
93+
var translation = loadTranslation2d(key);
94+
var rotation = loadRotation2d(key);
95+
if (translation.isEmpty() || rotation.isEmpty()) {
96+
return Optional.empty();
97+
}
98+
return Optional.of(new Pose2d(translation.get(), rotation.get()));
99+
}
100+
}

‎src/main/java/frc/lib/PoseLogger.java

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
package frc.lib;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.networktables.NetworkTable;
5+
import edu.wpi.first.networktables.NetworkTableInstance;
6+
import edu.wpi.first.networktables.StructPublisher;
7+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
8+
import java.util.HashMap;
9+
import java.util.Map;
10+
import java.util.Optional;
11+
import java.util.function.Supplier;
12+
13+
public class PoseLogger extends SubsystemBase {
14+
private static final PoseLogger defaultPosePublisher = new PoseLogger("Poses");
15+
16+
private record Info(Supplier<Optional<Pose2d>> supplier, StructPublisher<Pose2d> topic) {}
17+
18+
private final Map<String, Info> monitorPublishers = new HashMap<>();
19+
private final Map<String, StructPublisher<Pose2d>> publishers = new HashMap<>();
20+
private final NetworkTable table;
21+
22+
public PoseLogger(String groupName) {
23+
table = NetworkTableInstance.getDefault().getTable(groupName);
24+
}
25+
26+
public static PoseLogger getDefault() {
27+
return defaultPosePublisher;
28+
}
29+
30+
public void monitor(String name, Supplier<Optional<Pose2d>> poseSupplier) {
31+
monitorPublishers.put(
32+
name, new Info(poseSupplier, table.getStructTopic(name, Pose2d.struct).publish()));
33+
}
34+
35+
public synchronized void publish(String name, Pose2d pose) {
36+
var publisher = publishers.get(name);
37+
if (publisher == null) {
38+
publisher = table.getStructTopic(name, Pose2d.struct).publish();
39+
publishers.put(name, publisher);
40+
}
41+
publisher.set(pose);
42+
}
43+
44+
public void cancel(String name) {
45+
monitorPublishers.remove(name);
46+
}
47+
48+
@Override
49+
public void periodic() {
50+
monitorPublishers.forEach(
51+
(name, info) -> info.supplier.get().ifPresent(pose -> info.topic.set(pose)));
52+
}
53+
}

‎src/main/java/frc/lib/Util.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
import edu.wpi.first.wpilibj.DriverStation.Alliance;
44
import edu.wpi.first.wpilibj.util.Color;
55

6-
public class Util {
6+
public interface Util {
77
/**
88
* Map alliance object to corresponding color.
99
*

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import edu.wpi.first.units.measure.Distance;
1515
import edu.wpi.first.units.measure.LinearVelocity;
1616
import edu.wpi.first.units.measure.Time;
17+
import edu.wpi.first.wpilibj.Filesystem;
1718
import edu.wpi.first.wpilibj.TimedRobot;
1819
import edu.wpi.first.wpilibj.util.Color;
1920
import frc.game.FeederStation;
@@ -43,6 +44,8 @@ public static final class VisionConstants {
4344
// noise and how many or how frequently vision measurements are applied to the pose estimator.
4445
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
4546
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
47+
public static String kStemGymAprilTagLayoutPath =
48+
Filesystem.getDeployDirectory() + "/stemgym.json";
4649
}
4750

4851
public static final class RobotConstants {
@@ -90,6 +93,7 @@ public static final class AbsoluteEncoders {
9093
public static final AngularVelocity kMaxRotationalVelocity =
9194
RadiansPerSecond.of(5.0); // max 5.0
9295
public static final LinearVelocity kMinTranslationVelocity = MetersPerSecond.of(1.0);
96+
public static final LinearVelocity kMaxDriveToPoseTranslationVelocity = MetersPerSecond.of(1.0);
9397

9498
// The locations for the modules must be relative to the center of the robot.
9599
// Positive x values represent moving toward the front of the robot
@@ -250,7 +254,7 @@ public static final class ClimberConstants {
250254
public static final double kMaxAccelerationRPMPerSecond = kMaxVelocityRPM; // 100% accel in 1s
251255

252256
public static final double kDeployPosition = 7.0; // inches
253-
public static final double kRetractPosition = 1.25; // inches
257+
public static final double kRetractPosition = 2.25; // inches
254258
}
255259

256260
public static final class LedConstants {

0 commit comments

Comments
 (0)