VIsion and Roller

vision constants and implementation
roller percent output
operator controls update

src/main/java/frc4388/robot/RobotContainer.java

@@ -62,7 +62,7 @@ public class RobotContainer {
     /* Subsystems */
     public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
     //Testing of Colors
-    public final Vision m_vision = new Vision();
+    public final Vision m_vision = new Vision(m_robotMap.rightCamera);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
     public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
@@ -248,26 +248,26 @@ public class RobotContainer {
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
         //Operator Controls
-        new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
+        new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Extended);
             }));
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Retracted);
             }));
 
-        new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
+        new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
-            .onTrue(new InstantCommand(() -> {
-                m_robotShooter.setShooterReady();
-            }));
-        
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> {
                 m_robotShooter.setShooterNotReady();
             }));
         
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotShooter.setShooterReady();
+            }));
+
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Retracting);

src/main/java/frc4388/robot/RobotMap.java

@@ -9,6 +9,8 @@ package frc4388.robot;
 
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import org.photonvision.PhotonCamera;
+
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 
@@ -16,6 +18,7 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import frc4388.robot.constants.Constants;
 //import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.SimConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
 import frc4388.robot.subsystems.intake.IntakeConstants;
 import frc4388.robot.subsystems.intake.IntakeIO;
 import frc4388.robot.subsystems.intake.IntakeReal;
@@ -27,7 +30,10 @@ import frc4388.robot.subsystems.shooter.ShooterReal;
 import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
 import frc4388.robot.subsystems.swerve.SwerveIO;
 import frc4388.robot.subsystems.swerve.SwerveReal;
+import frc4388.robot.subsystems.vision.VisionIO;
+import frc4388.robot.subsystems.vision.VisionReal;
 import frc4388.utility.status.FaultCANCoder;
+import frc4388.utility.status.FaultPhotonCamera;
 import frc4388.utility.status.FaultPidgeon2;
 import frc4388.utility.status.FaultTalonFX;
 
@@ -40,7 +46,7 @@ public class RobotMap {
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
     // public final VisionIO leftCamera;
-    // public final VisionIO rightCamera;
+    public final VisionIO rightCamera;
 
     // public final LiDAR lidar = new 
 
@@ -63,13 +69,13 @@ public class RobotMap {
             case REAL:
                 // Configure cameras
                 // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
-                // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+                PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
 
                 // leftCamera =  new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS);                ;
-                // rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS);
+                rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
 
                 // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
-                // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
+                FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
 
                 // // Configure LiDAR
                 // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
@@ -131,7 +137,7 @@ public class RobotMap {
             //     break;
             default:
                 // leftCamera = new VisionIO() {};
-                // rightCamera = new VisionIO() {};
+                rightCamera = new VisionIO() {};
                 swerveDrivetrain = new SwerveIO() {};
                 intakeIO = new IntakeIO() {};
                 shooterIO = new ShooterIO() {};

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 61;
+  public static final int GIT_REVISION = 65;
-  public static final String GIT_SHA = "8551843831dbdc8790c0f9c02fb406248b69ec7a";
+  public static final String GIT_SHA = "7ec022bc7a2732fef46e2b0a7e5d5d31b88e1cde";
-  public static final String GIT_DATE = "2026-02-16 15:57:24 MST";
+  public static final String GIT_DATE = "2026-02-16 18:21:53 MST";
   public static final String GIT_BRANCH = "operator-controls";
-  public static final String BUILD_DATE = "2026-02-16 15:58:06 MST";
+  public static final String BUILD_DATE = "2026-02-17 19:45:30 MST";
-  public static final long BUILD_UNIX_TIME = 1771282686680L;
+  public static final long BUILD_UNIX_TIME = 1771382730050L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/constants/Constants.java

@@ -78,7 +78,7 @@ public final class Constants {
         public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
 
         public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
-        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(-13), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), new Rotation3d(0,40.*(Math.PI/180.),Math.PI));
         
         public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
 

src/main/java/frc4388/robot/constants/FieldConstants.java

@@ -4,23 +4,21 @@ import java.util.Arrays;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 
 public final class FieldConstants {
-    // public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+    public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
-    
-    public static final Translation2d BLUE_HUB_POS = new Translation2d();
-    public static final Translation2d RED_HUB_POS = new Translation2d();
     
     // Test april tag field layout
-        public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
+        // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
-            Arrays.asList(new AprilTag[] {
+        //     Arrays.asList(new AprilTag[] {
-                new AprilTag(0, new Pose3d(
+        //         new AprilTag(0, new Pose3d(
-                    new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
+        //             new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
-                )),
+        //         )),
-            }), 100, 100);
+        //     }), 100, 100);
 
 }

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -69,27 +69,27 @@ public class Intake extends SubsystemBase {
         switch (mode) {
             case Extended:
                 io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
-                io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
+                io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
                 break;
             case Retracted:
                 io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
-                io.setRollerVelocity(state, RotationsPerSecond.of(0));
+                io.setRollerOutput(state, 0);
                 break;
             case Extending:
                 io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
-                io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
+                io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
                 break;
             case Retracting:
                 io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
-                io.setRollerVelocity(state, RotationsPerSecond.of(0));
+                io.setRollerOutput(state, 0);
                 break;
             case Idle:
                 io.stopArm();
                 break;
         }
-        if (state.retractedLimit){
+        // if (state.retractedLimit){
-            this.mode = IntakeMode.Retracted;
+        //     this.mode = IntakeMode.Retracted;
-        }
+        // }
 
     }
 }

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -33,11 +33,11 @@ public class IntakeConstants {
     // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
     // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
 
-    public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3*0);
+    public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0);
-    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0);
+    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0*0.4);
     public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.1);
     public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.1);
-    public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
+    public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .10);
     // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
 
     // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
@@ -49,18 +49,13 @@ public class IntakeConstants {
         .withKI(0.0)
         .withKD(0.0);
 
-    public static final Slot0Configs ROLLER_PID = new Slot0Configs()
+    
-        .withKP(0.2)
-        .withKI(0.0)
-        .withKD(0.0);
 
     public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.1);
     public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
     public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);
     
-    public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
+
-    public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
-    public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
 
 
     // 0 is paralell to the ground, 90 is directly up

src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java

@@ -22,8 +22,8 @@ public interface IntakeIO {
         // Angle shooterTargetPitch = Rotations.of(0);
         // Current pitchMotorCurrent = Amps.of(0);
 
-        AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
+        double rollerOutput = 0;
-        AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
+        double rollerTargetOutput = 0;
         Current rollerMotorCurrent = Amps.of(0);
 
         // LinearVelocity feederVelocity = InchesPerSecond.of(0);
@@ -35,7 +35,7 @@ public interface IntakeIO {
     // public default void setShooterPitch(ShooterState state, Angle angle) {}
     public default void setArmAngle(IntakeState state, Angle angle) {}
     public default void stopArm() {}
-    public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
+    public default void setRollerOutput(IntakeState state, double rollerOutput) {}
     public default void armOutput(double percentOutput) {}
     public default void updateInputs(IntakeState state) {}
     public default void updateGains() {}

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -19,7 +19,7 @@ public class IntakeReal implements IntakeIO {
     DigitalInput m_armLimitSwitch;
 
     PositionDutyCycle armPosition = new PositionDutyCycle(0);
-    VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
+    // VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
 
     public IntakeReal(
         DigitalInput armLimitSwitch,
@@ -35,11 +35,10 @@ public class IntakeReal implements IntakeIO {
         // Apply the configs
         m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
         m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
-        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
         m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
 
         armPosition.Slot = 0;
-        rollerVelocity.Slot = 0;
+        // rollerVelocity.Slot = 0;
     }
 
     private Angle clampAng(Angle x, Angle min, Angle max){
@@ -55,20 +54,15 @@ public class IntakeReal implements IntakeIO {
 
     
     @Override
-    public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
+    public void setRollerOutput(IntakeState state, double rollerOutput) {
-        state.rollerTargetVelocity = angularVelocity;
+        state.rollerTargetOutput = rollerOutput;
 
-        if(angularVelocity.baseUnitMagnitude() == 0) {
+
+        if(rollerOutput == 0) {
             m_rollerMotor.set(0);
             return;
         }
-
+        m_rollerMotor.set(rollerOutput);
-        // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
-        AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
-
-        // m_rollerMotor.set(motorSpeed);
-        // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
-        m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed));
     }
 
     @Override
@@ -109,9 +103,14 @@ public class IntakeReal implements IntakeIO {
     public void updateInputs(IntakeState state) {
         state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
         state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
-        state.retractedLimit = !m_armLimitSwitch.get();
+        state.rollerOutput = m_rollerMotor.get();
-        state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
         state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
+        state.retractedLimit = !m_armLimitSwitch.get();
+
+        if(state.retractedLimit) {
+            // Set the arm motor to be zero if the limit switch is pressed
+            m_armMotor.setPosition(0., 0); 
+        }
     }
 
     @Override 
@@ -122,9 +121,5 @@ public class IntakeReal implements IntakeIO {
         IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
         m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
 
-        IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get();
-        IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get();
-        IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get();
-        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
     }
 }

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -25,8 +25,8 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35);
     public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10);
     
-    public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD Velocity", -10);
+    public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.1);
-    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse Velocity", 10);
+    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.1);
 
 
     // Tolerances
@@ -48,16 +48,10 @@ public class    ShooterConstants {
         .withKI(0.1)
         .withKD(0.08);
 
-    public static Slot0Configs INDEXER_PID = new Slot0Configs()
-        .withKV(0.0)
-        .withKP(0.0)
-        .withKI(0.0)
-        .withKD(0.2);
     
     
-    public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
+    
-    public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0);
+    
-    public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0);
     
     public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
     public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1);

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -13,7 +13,7 @@ public class ShooterReal implements ShooterIO {
 
     VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
     VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
-    VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
+    // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
 
 
     public ShooterReal(
@@ -27,7 +27,6 @@ public class ShooterReal implements ShooterIO {
 
         m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
         m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
-        m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
 
         m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
         m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
@@ -35,7 +34,7 @@ public class ShooterReal implements ShooterIO {
 
         shooter1Velocity.Slot = 0;
         shooter2Velocity.Slot = 0;
-        m_indexerVelocity.Slot = 0;
+        // m_indexerVelocity.Slot = 0;
     }
     
     @Override
@@ -95,11 +94,6 @@ public class ShooterReal implements ShooterIO {
         ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get();
         m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
         m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
-
-        ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get();
-        ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get();
-        ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get();
-        m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
     }
     
 }