third commit

src/main/java/frc/robot/Robot.java

@@ -89,19 +89,11 @@ public class Robot extends TimedRobot {
         // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run();
 
-<<<<<<< HEAD
-        Constants.RampConstants.getRampMotorSpeed();
-        Constants.IntakeConstants.getIntakeWheelsSpeed();
-        Constants.ShooterConstants.getShooterVelocity();
-        Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
-        distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
-=======
-       // Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
+        //Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
         Constants.IntakeConstants.getIntakeWheelsSpeed();
         Constants.ShooterConstants.getShooterVelocity();
         Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
         //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
     }
 
     /**

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -3,10 +3,7 @@ package frc.robot.subsystems;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.wpilibj.Servo;
-<<<<<<< HEAD
-=======
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -41,11 +38,6 @@ public class ClimberSubsystem extends SubsystemBase{
 
     public static void toggleRatchet(boolean toggle) {
     if (toggle == true) {
-<<<<<<< HEAD
-      climberRatchet.set(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
-    } else
-      climberRatchet.set(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
-=======
       climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
     } else
       climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
@@ -59,6 +51,5 @@ public class ClimberSubsystem extends SubsystemBase{
   public void periodic()
   {
     SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
   }
 }

src/main/java/frc/robot/subsystems/TargetingSubsystems.java

@@ -38,10 +38,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-<<<<<<< HEAD
-=======
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -49,13 +46,7 @@ import frc.robot.RobotContainer;
 import frc.robot.Constants;
 
 public class TargetingSubsystems extends SubsystemBase {
-<<<<<<< HEAD
-
-    RobotContainer m_RobotContainer = new RobotContainer();
-
-=======
     
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
     PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
     Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
     PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
@@ -71,17 +62,10 @@ public class TargetingSubsystems extends SubsystemBase {
 
     public List<Waypoint> rightClimbWaypoints;
 
-<<<<<<< HEAD
-    public Command pathPlanToRightClimbPoseCommand() {
-        GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
-        PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
-        currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose();
-=======
     public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
         GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
         PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
         currentRobotPose = swerveDrive.getPose();
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
         rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
                 currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
 
@@ -89,21 +73,12 @@ public class TargetingSubsystems extends SubsystemBase {
                 goalEndState);
         goToClimbPath.preventFlipping = true;
 
-<<<<<<< HEAD
-        return m_RobotContainer.getSwerveDriveBase().getAutonomousCommand("goToClimbPath");
-    }
-
-    public Command aimAndRangeToPose(Pose2d desiredPose) {
-        return new RunCommand(() -> {
-         currentRobotPose = m_RobotContainer.getSwerveDriveBase().getPose();
-=======
         return swerveDrive.getAutonomousCommand("goToClimbPath");
     }
 
     public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
         return new RunCommand(() -> {
          currentRobotPose = swerveDrive.getPose();
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
 
             Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
 
@@ -122,20 +97,12 @@ public class TargetingSubsystems extends SubsystemBase {
             double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
                     desiredPose.getRotation().getRadians());
             
-<<<<<<< HEAD
-            m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
-        }, m_RobotContainer.getSwerveDriveBase());
-    }
-
-    Command photonAimAtClimb = new RunCommand(() -> {
-=======
             swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
         }, swerveDrive);
     }
 
     Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { 
         return new RunCommand(() -> {
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
         double rot = 0.0;
         var result = photonVision.getLatestResult();
         if (result.hasTargets()) {
@@ -145,16 +112,10 @@ public class TargetingSubsystems extends SubsystemBase {
 
         rot = MathUtil.clamp(rot, -3.0, 3.0);
 
-<<<<<<< HEAD
-        m_RobotContainer.getSwerveDriveBase().drive(new Translation2d(m_RobotContainer.getDriverXbox().getLeftY() * -1,
-                m_RobotContainer.getDriverXbox().getLeftX() * -1), rot, true);
-    }, m_RobotContainer.getSwerveDriveBase());
-=======
         swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
                 driverXbox.getLeftX() * -1), rot, true);
     }, swerveDrive);
 }
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
 
 
     public PhotonPoseEstimator getPhotonPoseEstimator() {
@@ -190,18 +151,6 @@ public class TargetingSubsystems extends SubsystemBase {
         return distanceFromLimelightToGoalInches;
     }
 
-<<<<<<< HEAD
-    @Override
-    public void periodic() {
-
-        Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
-
-        if (result.isPresent()) {
-            EstimatedRobotPose estimatedPose = result.get();
-            m_RobotContainer.getSwerveDriveBase().getSwerveDrive()
-                    .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
-        }
-=======
     public void updateRobotPose(SwerveSubsystem swerveDrive) {
          Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
 
@@ -214,7 +163,6 @@ public class TargetingSubsystems extends SubsystemBase {
     @Override
     public void periodic() {
 
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
         /*
          * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
          * photonVision.getLatestResult().getBestTarget().getYaw());
@@ -229,11 +177,6 @@ public class TargetingSubsystems extends SubsystemBase {
          * "Arducam_OV9281_USB_Camera",
          * "http://photonvision.local:5800");
          */
-<<<<<<< HEAD
-    }
-}
-=======
     } 
 }
 
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f

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

@@ -62,11 +62,7 @@ public class SwerveSubsystem extends SubsystemBase {
     /**
      * Enable vision odometry updates while driving.
      */
-<<<<<<< HEAD
-    private final boolean visionDriveTest = true;
-=======
     private final boolean visionDriveTest = false;
->>>>>>> a50d67d7f53337b144e2b1afb5f7b644747fb21f
 
     /**
      * PhotonVision class to keep an accurate odometry.