pose localization trouble

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

@@ -43,7 +43,7 @@ import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-  //  private PhotonVision vision;
+    private PhotonVision vision;
     private Drivetrain drivetrain;
     private Hood hood;
     private Shooter shooter;
@@ -60,7 +60,7 @@ public class RobotContainer {
     private Timer shiftTimer;
 
     public RobotContainer() {
-        //vision = new PhotonVision();
+        vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
         hood = new Hood();
         shooter = new Shooter();
@@ -70,7 +70,7 @@ public class RobotContainer {
         //climber = new Climber();
         configureNamedCommands();
 
-        /* 
+         
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
@@ -78,7 +78,7 @@ public class RobotContainer {
                 vp.visualPose()
             );
         });
-        */
+        
 
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);

src/main/java/frc/robot/constants/IntakeRollerConstants.java

@@ -10,8 +10,8 @@ public class IntakeRollerConstants {
 
     public static final int kCurrentLimit = 65;
 
-    public static final boolean kInvertLeftMotor = true;
-    public static final boolean kInvertRightMotor = false;
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;
 
     public static final double kSpeed = 1;
 
@@ -31,6 +31,6 @@ public class IntakeRollerConstants {
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kInvertRightMotor)
-            .follow(kLeftMotorCANID);
+        ;
     }
 }

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -12,15 +12,24 @@ public class PhotonConstants {
     public static final String kCamera2Name = "CameraPV2";
 
     // TODO Need actual values for all of this
-    public static final Transform3d kCamera1RobotToCam = new Transform3d();
+    public static final Transform3d kCamera1RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-8.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(24.0), 
+            Units.degreesToRadians(30.0)
+        )
+    );
     public static final Transform3d kCamera2RobotToCam = new Transform3d(
         Units.inchesToMeters(1.5),
         Units.inchesToMeters(-10.5),
         Units.inchesToMeters(28.5),
         new Rotation3d(
-            Units.degreesToRadians(0), 
-            Units.degreesToRadians(24), 
-            Units.degreesToRadians(-10)
+            Units.degreesToRadians(0.0), 
+            Units.degreesToRadians(24.0), 
+            Units.degreesToRadians(-10.0)
         )
     );
 
@@ -32,7 +41,7 @@ public class PhotonConstants {
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final List<PhotonVisionConfig> configs = List.of(
-        //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
         new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
     );
 }

src/main/java/frc/robot/constants/SpindexerConstants.java

@@ -19,7 +19,7 @@ public class SpindexerConstants {
     public static final double kSpindexerSpeed = 1;
     public static final double kFeederSpeed = 1;
 
-    public static final boolean kFeederMotorInverted = true;
+    public static final boolean kFeederMotorInverted = false;
 
     public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
     public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;

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

@@ -332,6 +332,10 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public void consumeVisualPose(VisualPose pose) {
+        if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) {
+            return;
+        }
+        
         estimator.addVisionMeasurement(
             pose.visualPose(), 
             pose.timestamp()
@@ -376,7 +380,8 @@ public class Drivetrain extends SubsystemBase {
         SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative ?
                 ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    estimator.getEstimatedPosition().getRotation()) :
+                    //estimator.getEstimatedPosition().getRotation()) :
+                    Rotation2d.fromDegrees(getGyroValue())) :
                 new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
         );
 

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

@@ -33,18 +33,21 @@ public class IntakeRoller extends SubsystemBase {
     public Command runIn() {
         return run(() -> {
             leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.8);
         });
     }
 
     public Command runOut() {
         return run(() -> {
             leftMotor.set(-IntakeRollerConstants.kSpeed);
+            rightMotor.set(-IntakeRollerConstants.kSpeed);
         });
     }
 
     public Command stop() {
         return run(() -> {
             leftMotor.set(0);
+            rightMotor.set(0);
         });
     }