Fix small merge issue with drivetrain

redshiftrobotics · Mar 2, 2024 · 62815f9 · 62815f9
1 parent 821abe6
commit 62815f9
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 18 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -87,8 +87,10 @@ public class RobotContainer {
 			ArmConstants.RIGHT_ENCODER_ID,
 			ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm();
 
-	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
-			swerveModuleBL, swerveModuleBR);
+	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(
+		gyro,
+		swerveModuleFL, swerveModuleFR,
+		swerveModuleBL, swerveModuleBR);
 
 	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -1,13 +1,10 @@
 package frc.robot.subsystems;
 
 import java.util.Arrays;
-import java.util.Optional;
 import java.util.function.Consumer;
 import java.util.function.Function;
 import java.util.function.IntFunction;
 
-import org.photonvision.EstimatedRobotPose;
-
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.math.controller.PIDController;
@@ -58,11 +55,6 @@ public class SwerveDrivetrain extends SubsystemBase {
 	 */
 	private final AHRS gyro;
 
-	/**
-	 * Vision subsystem of robot for pose estimator
-	 */
-	private final Vision visionSystem;
-
 	/**
 	 * Pose of robot. The pose is the current the X, Y and Rotation position of the robot, relative to the last reset.
 	 * It is updated every 20ms in periodic.
@@ -91,13 +83,13 @@ public class SwerveDrivetrain extends SubsystemBase {
 	 * @param swerveModuleBL Back left swerve module
 	 * @param swerveModuleBR Back right swerve module
 	 */
-	public SwerveDrivetrain(AHRS gyro, Vision vision,
+	public SwerveDrivetrain(
+		AHRS gyro,
 		SwerveModule swerveModuleFL, SwerveModule swerveModuleFR,
 		SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
 
 		// save parameters
 		this.gyro = gyro;
-		this.visionSystem = vision;
 
 		moduleFL = swerveModuleFL;
 		moduleFR = swerveModuleFR;
@@ -143,12 +135,12 @@ public void periodic() {
 			getHeading(),
 			getWheelPositions());
 
-		if (visionSystem != null) {
-			Optional<EstimatedRobotPose> estimatedPose = visionSystem.getEstimatedGlobalPose();
-			if (estimatedPose.isPresent()) {
-				poseEstimator.addVisionMeasurement(estimatedPose.get().estimatedPose.toPose2d(), estimatedPose.get().timestampSeconds);
-			}
-		}
+		// if (visionSystem != null) {
+		// 	Optional<EstimatedRobotPose> estimatedPose = visionSystem.getEstimatedGlobalPose();
+		// 	if (estimatedPose.isPresent()) {
+		// 		poseEstimator.addVisionMeasurement(estimatedPose.get().estimatedPose.toPose2d(), estimatedPose.get().timestampSeconds);
+		// 	}
+		// }
 
 		if (desiredPose != null) {
 			// Calculate our robot speeds with the PID controllers