diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 058282b..a034180 100644 --- 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 autoChooser = new SendableChooser(); diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 1fc4f64..da14047 100644 --- 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 estimatedPose = visionSystem.getEstimatedGlobalPose(); - if (estimatedPose.isPresent()) { - poseEstimator.addVisionMeasurement(estimatedPose.get().estimatedPose.toPose2d(), estimatedPose.get().timestampSeconds); - } - } + // if (visionSystem != null) { + // Optional 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