Skip to content

Commit dd97944

Browse files
lessfoodlessfood
committed
vision
1 parent f2eaf94 commit dd97944

File tree

8 files changed

+156
-74
lines changed

8 files changed

+156
-74
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,7 @@ test {
108108
// The sim GUI is *disabled* by default to support running
109109
// AdvantageKit log replay, which requires that all HAL
110110
// sim extensions be disabled.
111-
wpi.sim.addGui().defaultEnabled = false
111+
wpi.sim.addGui().defaultEnabled = true
112112
wpi.sim.addDriverstation()
113113

114114
// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

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

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -178,6 +178,14 @@ private void configureButtonBindings() {
178178
new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)),
179179
drive)
180180
.ignoringDisable(true));
181+
controller
182+
.y()
183+
.whileTrue(
184+
DriveCommands.lookAt(
185+
drive,
186+
() -> -controller.getLeftY(),
187+
() -> -controller.getLeftX(),
188+
() -> vision.getTargetX(28)));
181189
}
182190

183191
/**

‎src/main/java/frc/robot/commands/Align.java‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,21 @@
55
package frc.robot.commands;
66

77
import edu.wpi.first.wpilibj2.command.Command;
8+
import frc.robot.subsystems.drive.Drive;
9+
import frc.robot.subsystems.vision.Vision;
810

911
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
1012
public class Align extends Command {
1113
/** Creates a new Align. */
12-
public Align() {
14+
public Drive drive;
15+
16+
public Vision vision;
17+
18+
public Align(Drive _drive, Vision _vision) {
1319
// Use addRequirements() here to declare subsystem dependencies.
20+
drive = _drive;
21+
vision = _vision;
22+
addRequirements(drive, vision);
1423
}
1524

1625
// Called when the command is initially scheduled.

‎src/main/java/frc/robot/commands/DriveCommands.java‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -150,6 +150,58 @@ public static Command joystickDriveAtAngle(
150150
.beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
151151
}
152152

153+
/**
154+
* pid look at thingy
155+
*/
156+
public static Command lookAt(
157+
Drive drive,
158+
DoubleSupplier xSupplier,
159+
DoubleSupplier ySupplier,
160+
Supplier<Rotation2d> rotationSupplier) {
161+
162+
// Create PID controller
163+
ProfiledPIDController angleController =
164+
new ProfiledPIDController(
165+
1,
166+
0,
167+
0.02,
168+
new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
169+
angleController.enableContinuousInput(-Math.PI, Math.PI);
170+
171+
// Construct command
172+
return Commands.run(
173+
() -> {
174+
// Get linear velocity
175+
Translation2d linearVelocity =
176+
getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble());
177+
178+
// Calculate angular speed
179+
double omega =
180+
angleController.calculate(
181+
drive.getRotation().getRadians(), rotationSupplier.get().getRadians());
182+
183+
// Convert to field relative speeds & send command
184+
ChassisSpeeds speeds =
185+
new ChassisSpeeds(
186+
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
187+
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
188+
omega);
189+
boolean isFlipped =
190+
DriverStation.getAlliance().isPresent()
191+
&& DriverStation.getAlliance().get() == Alliance.Red;
192+
drive.runVelocity(
193+
ChassisSpeeds.fromFieldRelativeSpeeds(
194+
speeds,
195+
isFlipped
196+
? drive.getRotation().plus(new Rotation2d(Math.PI))
197+
: drive.getRotation()));
198+
},
199+
drive)
200+
201+
// Reset PID controller when command starts
202+
.beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
203+
}
204+
153205
/**
154206
* Measures the velocity feedforward constants for the drive motors.
155207
*

‎src/main/java/frc/robot/subsystems/vision/Vision.java‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,19 @@ public Vision(VisionConsumer consumer, VisionIO... io) {
5555
* @param cameraIndex The index of the camera to use.
5656
*/
5757
public Rotation2d getTargetX(int cameraIndex) {
58-
return inputs[cameraIndex].latestTargetObservation.tx();
58+
// Add tag poses
59+
//for (int c = 0; c < 2; c++) {
60+
for (int tagId : inputs[0].tagIds) {
61+
// for (int tagId2 : inputs[1].tagIds) {
62+
63+
var tagPose = aprilTagLayout.getTagPose(tagId);
64+
// if (tagId == cameraIndex) {
65+
// DriverStation.reportWarning(Integer.toString(tagId), false);
66+
return tagPose.get().toPose2d().getRotation();
67+
//}
68+
}
69+
// }
70+
return Rotation2d.kZero;
5971
}
6072

6173
@Override

‎src/main/java/frc/robot/subsystems/vision/VisionConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ public class VisionConstants {
2626
public static Transform3d robotToCamera0 =
2727
new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
2828
public static Transform3d robotToCamera1 =
29-
new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
29+
new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
3030

3131
// Basic filtering thresholds
3232
public static double maxAmbiguity = 0.3;

‎src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ public VisionIOPhotonVisionSim(
4343
// Add sim camera
4444
var cameraProperties = new SimCameraProperties();
4545
cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout);
46+
cameraSim.enableDrawWireframe(true);
4647
visionSim.addCamera(cameraSim, robotToCamera);
4748
}
4849

‎vendordeps/photonlib.json‎

Lines changed: 70 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -1,71 +1,71 @@
11
{
2-
"fileName": "photonlib.json",
3-
"name": "photonlib",
4-
"version": "v2026.1.1-rc-3",
5-
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
6-
"frcYear": "2026",
7-
"mavenUrls": [
8-
"https://maven.photonvision.org/repository/internal",
9-
"https://maven.photonvision.org/repository/snapshots"
10-
],
11-
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
12-
"jniDependencies": [
13-
{
14-
"groupId": "org.photonvision",
15-
"artifactId": "photontargeting-cpp",
16-
"version": "v2026.1.1-rc-3",
17-
"skipInvalidPlatforms": true,
18-
"isJar": false,
19-
"validPlatforms": [
20-
"windowsx86-64",
21-
"linuxathena",
22-
"linuxx86-64",
23-
"osxuniversal"
24-
]
25-
}
26-
],
27-
"cppDependencies": [
28-
{
29-
"groupId": "org.photonvision",
30-
"artifactId": "photonlib-cpp",
31-
"version": "v2026.1.1-rc-3",
32-
"libName": "photonlib",
33-
"headerClassifier": "headers",
34-
"sharedLibrary": true,
35-
"skipInvalidPlatforms": true,
36-
"binaryPlatforms": [
37-
"windowsx86-64",
38-
"linuxathena",
39-
"linuxx86-64",
40-
"osxuniversal"
41-
]
42-
},
43-
{
44-
"groupId": "org.photonvision",
45-
"artifactId": "photontargeting-cpp",
46-
"version": "v2026.1.1-rc-3",
47-
"libName": "photontargeting",
48-
"headerClassifier": "headers",
49-
"sharedLibrary": true,
50-
"skipInvalidPlatforms": true,
51-
"binaryPlatforms": [
52-
"windowsx86-64",
53-
"linuxathena",
54-
"linuxx86-64",
55-
"osxuniversal"
56-
]
57-
}
58-
],
59-
"javaDependencies": [
60-
{
61-
"groupId": "org.photonvision",
62-
"artifactId": "photonlib-java",
63-
"version": "v2026.1.1-rc-3"
64-
},
65-
{
66-
"groupId": "org.photonvision",
67-
"artifactId": "photontargeting-java",
68-
"version": "v2026.1.1-rc-3"
69-
}
70-
]
71-
}
2+
"fileName": "photonlib.json",
3+
"name": "photonlib",
4+
"version": "v2026.1.1-rc-3",
5+
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
6+
"frcYear": "2026",
7+
"mavenUrls": [
8+
"https://maven.photonvision.org/repository/internal",
9+
"https://maven.photonvision.org/repository/snapshots"
10+
],
11+
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
12+
"jniDependencies": [
13+
{
14+
"groupId": "org.photonvision",
15+
"artifactId": "photontargeting-cpp",
16+
"version": "v2026.1.1-rc-3",
17+
"skipInvalidPlatforms": true,
18+
"isJar": false,
19+
"validPlatforms": [
20+
"windowsx86-64",
21+
"linuxathena",
22+
"linuxx86-64",
23+
"osxuniversal"
24+
]
25+
}
26+
],
27+
"cppDependencies": [
28+
{
29+
"groupId": "org.photonvision",
30+
"artifactId": "photonlib-cpp",
31+
"version": "v2026.1.1-rc-3",
32+
"libName": "photonlib",
33+
"headerClassifier": "headers",
34+
"sharedLibrary": true,
35+
"skipInvalidPlatforms": true,
36+
"binaryPlatforms": [
37+
"windowsx86-64",
38+
"linuxathena",
39+
"linuxx86-64",
40+
"osxuniversal"
41+
]
42+
},
43+
{
44+
"groupId": "org.photonvision",
45+
"artifactId": "photontargeting-cpp",
46+
"version": "v2026.1.1-rc-3",
47+
"libName": "photontargeting",
48+
"headerClassifier": "headers",
49+
"sharedLibrary": true,
50+
"skipInvalidPlatforms": true,
51+
"binaryPlatforms": [
52+
"windowsx86-64",
53+
"linuxathena",
54+
"linuxx86-64",
55+
"osxuniversal"
56+
]
57+
}
58+
],
59+
"javaDependencies": [
60+
{
61+
"groupId": "org.photonvision",
62+
"artifactId": "photonlib-java",
63+
"version": "v2026.1.1-rc-3"
64+
},
65+
{
66+
"groupId": "org.photonvision",
67+
"artifactId": "photontargeting-java",
68+
"version": "v2026.1.1-rc-3"
69+
}
70+
]
71+
}

0 commit comments

Comments
 (0)