Skip to content
This repository was archived by the owner on Dec 6, 2025. It is now read-only.

Commit 784757e

Browse files
thenetworkgrinchthenetworkgrinch
committed
Updated vendordeps and added back vision code.
Signed-off-by: thenetworkgrinch <[email protected]>
1 parent 45eea70 commit 784757e

File tree

8 files changed

+820
-729
lines changed

8 files changed

+820
-729
lines changed

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

Lines changed: 40 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -52,24 +52,37 @@ public class RobotContainer
5252
driverXbox::getRightY)
5353
.headingWhile(true);
5454

55-
56-
SwerveInputStream driveAngularVelocitySim = SwerveInputStream.of(drivebase.getSwerveDrive(),
57-
() -> -driverXbox.getLeftY(),
58-
() -> -driverXbox.getLeftX())
59-
.withControllerRotationAxis(() -> driverXbox.getRawAxis(2))
60-
.deadband(OperatorConstants.DEADBAND)
61-
.scaleTranslation(0.8)
62-
.allianceRelativeControl(true);
55+
/**
56+
* Clone's the angular velocity input stream and converts it to a robotRelative input stream.
57+
*/
58+
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true)
59+
.allianceRelativeControl(false);
60+
61+
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
62+
() -> -driverXbox.getLeftY(),
63+
() -> -driverXbox.getLeftX())
64+
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
65+
2))
66+
.deadband(OperatorConstants.DEADBAND)
67+
.scaleTranslation(0.8)
68+
.allianceRelativeControl(true);
6369
// Derive the heading axis with math!
64-
SwerveInputStream driveDirectAngleSim = driveAngularVelocitySim.copy()
65-
.withControllerHeadingAxis(() -> Math.sin(
66-
driverXbox.getRawAxis(
67-
2) * Math.PI) * (Math.PI * 2),
68-
() -> Math.cos(
69-
driverXbox.getRawAxis(
70-
2) * Math.PI) *
71-
(Math.PI * 2))
72-
.headingWhile(true);
70+
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
71+
.withControllerHeadingAxis(() ->
72+
Math.sin(
73+
driverXbox.getRawAxis(
74+
2) *
75+
Math.PI) *
76+
(Math.PI *
77+
2),
78+
() ->
79+
Math.cos(
80+
driverXbox.getRawAxis(
81+
2) *
82+
Math.PI) *
83+
(Math.PI *
84+
2))
85+
.headingWhile(true);
7386

7487
/**
7588
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -92,17 +105,19 @@ public RobotContainer()
92105
private void configureBindings()
93106
{
94107

95-
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
96-
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
97-
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle);
98-
Command driveFieldOrientedDirectAngleSim = drivebase.driveFieldOriented(driveDirectAngleSim);
99-
Command driveFieldOrientedAnglularVelocitySim = drivebase.driveFieldOriented(driveAngularVelocitySim);
100-
Command driveSetpointGenSim = drivebase.driveWithSetpointGeneratorFieldRelative(
101-
driveDirectAngleSim);
108+
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
109+
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
110+
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
111+
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
112+
driveDirectAngle);
113+
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
114+
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
115+
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
116+
driveDirectAngleKeyboard);
102117

103118
if (RobotBase.isSimulation())
104119
{
105-
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleSim);
120+
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
106121
} else
107122
{
108123
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);

‎src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java‎

Lines changed: 35 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
import edu.wpi.first.wpilibj2.command.SubsystemBase;
3737
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
3838
import frc.robot.Constants;
39-
//import frc.robot.subsystems.swervedrive.Vision.Cameras;
39+
import frc.robot.subsystems.swervedrive.Vision.Cameras;
4040
import java.io.File;
4141
import java.io.IOException;
4242
import java.util.Arrays;
@@ -45,7 +45,7 @@
4545
import java.util.function.DoubleSupplier;
4646
import java.util.function.Supplier;
4747
import org.json.simple.parser.ParseException;
48-
//import org.photonvision.targeting.PhotonPipelineResult;
48+
import org.photonvision.targeting.PhotonPipelineResult;
4949
import swervelib.SwerveController;
5050
import swervelib.SwerveDrive;
5151
import swervelib.SwerveDriveTest;
@@ -74,7 +74,7 @@ public class SwerveSubsystem extends SubsystemBase
7474
/**
7575
* PhotonVision class to keep an accurate odometry.
7676
*/
77-
// private Vision vision;
77+
private Vision vision;
7878

7979
/**
8080
* Initialize {@link SwerveDrive} with the directory provided.
@@ -107,7 +107,7 @@ public SwerveSubsystem(File directory)
107107
// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible
108108
if (visionDriveTest)
109109
{
110-
// setupPhotonVision();
110+
setupPhotonVision();
111111
// Stop the odometry thread if we are using vision that way we can synchronize updates better.
112112
swerveDrive.stopOdometryThread();
113113
}
@@ -129,13 +129,13 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
129129
Rotation2d.fromDegrees(0)));
130130
}
131131

132-
// /**
133-
// * Setup the photon vision class.
134-
// */
135-
// public void setupPhotonVision()
136-
// {
137-
// vision = new Vision(swerveDrive::getPose, swerveDrive.field);
138-
// }
132+
/**
133+
* Setup the photon vision class.
134+
*/
135+
public void setupPhotonVision()
136+
{
137+
vision = new Vision(swerveDrive::getPose, swerveDrive.field);
138+
}
139139

140140
@Override
141141
public void periodic()
@@ -144,7 +144,7 @@ public void periodic()
144144
if (visionDriveTest)
145145
{
146146
swerveDrive.updateOdometry();
147-
// vision.updatePoseEstimation(swerveDrive);
147+
vision.updatePoseEstimation(swerveDrive);
148148
}
149149
}
150150

@@ -224,29 +224,29 @@ public void setupPathPlanner()
224224
PathfindingCommand.warmupCommand().schedule();
225225
}
226226

227-
// /**
228-
// * Aim the robot at the target returned by PhotonVision.
229-
// *
230-
// * @return A {@link Command} which will run the alignment.
231-
// */
232-
// public Command aimAtTarget(Cameras camera)
233-
// {
234-
//
235-
// return run(() -> {
236-
// Optional<PhotonPipelineResult> resultO = camera.getBestResult();
237-
// if (resultO.isPresent())
238-
// {
239-
// var result = resultO.get();
240-
// if (result.hasTargets())
241-
// {
242-
// drive(getTargetSpeeds(0,
243-
// 0,
244-
// Rotation2d.fromDegrees(result.getBestTarget()
245-
// .getYaw()))); // Not sure if this will work, more math may be required.
246-
// }
247-
// }
248-
// });
249-
// }
227+
/**
228+
* Aim the robot at the target returned by PhotonVision.
229+
*
230+
* @return A {@link Command} which will run the alignment.
231+
*/
232+
public Command aimAtTarget(Cameras camera)
233+
{
234+
235+
return run(() -> {
236+
Optional<PhotonPipelineResult> resultO = camera.getBestResult();
237+
if (resultO.isPresent())
238+
{
239+
var result = resultO.get();
240+
if (result.hasTargets())
241+
{
242+
drive(getTargetSpeeds(0,
243+
0,
244+
Rotation2d.fromDegrees(result.getBestTarget()
245+
.getYaw()))); // Not sure if this will work, more math may be required.
246+
}
247+
}
248+
});
249+
}
250250

251251
/**
252252
* Get the path follower with events.

0 commit comments

Comments
 (0)