diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 756eb89..29d3ec1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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,25 +248,25 @@ 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) - .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReady(); - })); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) .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(() -> { diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a1c423a..3efa9c3 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() {}; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 8f37937..45e4837 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 = 61; - public static final String GIT_SHA = "8551843831dbdc8790c0f9c02fb406248b69ec7a"; - public static final String GIT_DATE = "2026-02-16 15:57:24 MST"; + public static final int GIT_REVISION = 65; + public static final String GIT_SHA = "7ec022bc7a2732fef46e2b0a7e5d5d31b88e1cde"; + 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 long BUILD_UNIX_TIME = 1771282686680L; + public static final String BUILD_DATE = "2026-02-17 19:45:30 MST"; + public static final long BUILD_UNIX_TIME = 1771382730050L; 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 efae75f..dc76b83 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/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 diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index 68683f6..f5a7245 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/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( - Arrays.asList(new AprilTag[] { - new AprilTag(0, new Pose3d( - new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - )), - }), 100, 100); + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(0, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 1b5d74c..bed41fe 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/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){ - this.mode = IntakeMode.Retracted; - } + // if (state.retractedLimit){ + // this.mode = IntakeMode.Retracted; + // } } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 713e9d9..f1c9d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/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_EXTENDED = new ConfigurableDouble("Arm angle extended", 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*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 diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 4babe7f..2568ec6 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/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); - AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + double rollerOutput = 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() {} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 9f7359f..7546e58 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/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) { - state.rollerTargetVelocity = angularVelocity; + public void setRollerOutput(IntakeState state, double rollerOutput) { + state.rollerTargetOutput = rollerOutput; - if(angularVelocity.baseUnitMagnitude() == 0) { + + if(rollerOutput == 0) { m_rollerMotor.set(0); return; } - - // (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)); + m_rollerMotor.set(rollerOutput); } @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.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + state.rollerOutput = m_rollerMotor.get(); 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); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 55e533c..ef06902 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/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_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse 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 % 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); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 4379de6..04fb459 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/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); } }