From 9c7159ba3b2fd669f9781f1315cb481b7a1b8d57 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Fri, 20 Feb 2026 20:59:06 -0800 Subject: [PATCH] left camera --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 10 ++--- .../robot/constants/BuildConstants.java | 10 ++--- .../frc4388/robot/constants/Constants.java | 10 ++--- .../robot/subsystems/intake/Intake.java | 43 ++++++++++--------- .../subsystems/shooter/ShooterConstants.java | 24 ++++++++--- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- .../robot/subsystems/vision/Vision.java | 8 ++-- .../robot/subsystems/vision/VisionReal.java | 2 +- 9 files changed, 62 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e342c3d..334d3c4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -63,7 +63,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(m_robotMap.rightCamera); + public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); 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); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3efa9c3..9791edb 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -45,7 +45,7 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - // public final VisionIO leftCamera; + public final VisionIO leftCamera; public final VisionIO rightCamera; // public final LiDAR lidar = new @@ -68,13 +68,13 @@ public class RobotMap { switch (mode) { case REAL: // Configure cameras - // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - // leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; + leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); - // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // // Configure LiDAR @@ -136,7 +136,7 @@ public class RobotMap { // case SIM: // break; default: - // leftCamera = new VisionIO() {}; + leftCamera = new VisionIO() {}; rightCamera = new VisionIO() {}; swerveDrivetrain = new SwerveIO() {}; intakeIO = new IntakeIO() {}; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 1704484..f18e71c 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/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 = 69; - public static final String GIT_SHA = "9bf368bf4a5ca480b13aece556686ad0270f44b0"; - public static final String GIT_DATE = "2026-02-18 17:37:49 MST"; + public static final int GIT_REVISION = 71; + public static final String GIT_SHA = "32ede81ffaf43d4e730ff646db98627385e80ba1"; + public static final String GIT_DATE = "2026-02-20 16:24:05 MST"; public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-19 17:17:10 MST"; - public static final long BUILD_UNIX_TIME = 1771546630658L; + public static final String BUILD_DATE = "2026-02-20 21:42:37 MST"; + public static final long BUILD_UNIX_TIME = 1771648957538L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index dd42952..b596e5a 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -78,14 +78,14 @@ 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)); + new Translation3d(Units.inchesToMeters(-13-9.134-1.5), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5) + ), new Rotation3d(0,25.*(Math.PI/180.),Math.PI)); 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,50.*(Math.PI/180.),0) + new Translation3d(Units.inchesToMeters(-13-9.134-1.5), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), + new Rotation3d(0,25.*(Math.PI/180.),Math.PI) ); - public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters + public static final double MIN_ESTIMATION_DISTANCE = 5; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 606334b..e9e5868 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -63,30 +63,31 @@ public class Intake extends SubsystemBase { Logger.processInputs("Intake", state); + Logger.recordOutput("Intake/IntakeState", this.mode); io.updateInputs(state); - switch (mode) { - case Extended: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - break; - case Retracted: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - io.setRollerOutput(state, 0); - break; - case Extending: - io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - break; - case Retracting: - io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, 0); - break; - case Idle: - io.stopArm(); - break; - } + // switch (mode) { + // case Extended: + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + // break; + // case Retracted: + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + // io.setRollerOutput(state, 0); + // break; + // case Extending: + // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + // break; + // case Retracting: + // io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); + // io.setRollerOutput(state, 0); + // break; + // case Idle: + // io.stopArm(); + // break; + // } // if (state.retractedLimit){ // this.mode = IntakeMode.Retracted; // } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 360be28..f6ff699 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -19,9 +19,10 @@ public class ShooterConstants { public static final double INDEXER_GEAR_RATIO = 1.; public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); + public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); - public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.1); + public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); @@ -39,17 +40,30 @@ public class ShooterConstants { // public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { - double speed = SHOOTER_ACTIVE_VELOCITY.get(); + // Model derived from points + double speed = + 1.11576*hubDistMeters*hubDistMeters + + 0.318464*hubDistMeters + + 30.6293; + + double max = SHOOTER_MAX_VELOCITY.get(); - return RotationsPerSecond.of(speed); + // Clamp speed to max + if(speed > max) { + speed = max; + } else if(speed < -max) { + speed = -max; + } + + return RotationsPerSecond.of(-speed); } // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.0) + .withKP(0.08) .withKI(0.1) - .withKD(0.08); + .withKD(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index fb1d78e..cb7fcd1 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -351,7 +351,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); // Calculate the angle between the current position and the lead position - Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); driveFieldAngle(leftStick, ang); diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index 9ab223e..bc6aae9 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -23,13 +23,11 @@ public class Vision extends SubsystemBase implements Queryable{ public Pose2d lastVisionPose = new Pose2d(); public Pose2d lastPhysOdomPose = new Pose2d(); - public LED m_robotLED; public Vision(VisionIO... devices) { FaultReporter.register(this); io = devices; state = new VisionStateAutoLogged[io.length]; - m_robotLED = new LED(); for(int i = 0; i < io.length; i++) { state[i] = new VisionStateAutoLogged(); } @@ -43,9 +41,9 @@ public class Vision extends SubsystemBase implements Queryable{ } Logger.recordOutput("Vision/isTagDectected", isTag()); - if (isTag()){ - m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK); - } + // if (isTag()){ + // m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK); + // } } public List getPosesToAdd(){ diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 6f70542..8c4ba31 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -49,7 +49,7 @@ public class VisionReal implements VisionIO { if(!result.hasTargets()) return; - state.isTagDetected = state.isTagDetected | result.hasTargets(); + state.isTagDetected = result.hasTargets(); Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator);