diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f472457..0d47677 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -63,7 +63,7 @@ private Command shoot() { m_shooter.runShooter(), Commands.waitUntil(m_shooter.readyToShoot) .andThen(m_feeder.shoot().alongWith(m_intake.intake()).withTimeout(0.5))) - .andThen(m_shooter.stop(), m_feeder.stop()) + .andThen(m_shooter.stop(), m_feeder.stop(), m_intake.stop()) .finallyDo(() -> System.out.println("Shooting complete!")); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b6ecd9..6c9c7df 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,7 +10,7 @@ import java.util.function.BooleanSupplier; public final class Constants { - public static final Mode kCurrentMode = Mode.kReal; + public static final Mode kCurrentMode = Mode.kSim; public static final RobotName kRobot = RobotName.kViper; public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim); public static final String kDrivetrainCanBus = "CANivore1"; @@ -20,11 +20,11 @@ public final class Constants { () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - public static final boolean useWpiFieldLayout = true; + private static final boolean kUseWpiFieldLayout = false; public static final AprilTagFieldLayout fieldLayout; static { - if (useWpiFieldLayout) { + if (kUseWpiFieldLayout) { try { fieldLayout = new AprilTagFieldLayout( @@ -35,9 +35,7 @@ public final class Constants { } catch (IOException e) { throw new RuntimeException(e); } - } else - fieldLayout = - useWpiFieldLayout ? null : AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); + } else fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); } public static enum Mode { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 30f654e..c8f6add 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,7 +20,6 @@ import frc.robot.shooter.Shooter; import frc.robot.vision.Vision; import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -67,7 +66,6 @@ public Robot() { } Logger.start(); - LogTable.disableProtobufWarning(); switch (Constants.kCurrentMode) { case kReal: @@ -82,11 +80,11 @@ public Robot() { case kSim: m_drivetrain = Subsystems.createSimDrivetrain(); m_vision = Subsystems.createFourCameraVision(); - m_intake = Subsystems.createBlankIntake(); + m_intake = Subsystems.createSimIntake(); m_arm = Subsystems.createSimArm(); m_climber = Subsystems.createBlankClimber(); m_shooter = Subsystems.createSimShooter(); - m_feeder = Subsystems.createBlankFeeder(); + m_feeder = Subsystems.createSimFeeder(); break; default: m_drivetrain = Subsystems.createBlankDrivetrain(); @@ -102,7 +100,7 @@ public Robot() { NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier); NoteVisualizer.resetNotes(); NoteVisualizer.showStagedNotes(); - Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake); + final Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake); NamedCommands.registerCommand("intake", m_intake.intake()); NamedCommands.registerCommand("intakeOff", m_intake.idle()); NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter())); @@ -195,8 +193,6 @@ public Robot() { m_driverController.start().onTrue(m_drivetrain.zeroGyro()); m_driverController.leftStick().toggleOnTrue(m_arm.aimElbowForTuning()); m_driverController.rightStick().toggleOnTrue(m_arm.aimWristForTuning()); - // m_driverController.a().whileTrue(m_climber.unwindWinch()); - // m_driverController.y().whileTrue(m_climber.windWinch()); m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake)); m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb)); m_driverController.b().whileTrue(Commands.parallel(m_arm.idleCoast(), m_climber.windWinch())); diff --git a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java index d70fed8..df8726f 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java @@ -12,15 +12,15 @@ public class ImuIOPigeon2 implements ImuIO { private final Pigeon2 m_imu = new Pigeon2(20, Constants.kDrivetrainCanBus); - private final StatusSignal yaw = m_imu.getYaw(); + private final StatusSignal m_yawSignal = m_imu.getYaw(); + private final StatusSignal m_yawVelocitySignal = m_imu.getAngularVelocityZWorld(); private final Queue m_yawPositionQueue, m_yawTimestampQueue; - private final StatusSignal yawVelocity = m_imu.getAngularVelocityZWorld(); public ImuIOPigeon2() { m_imu.getConfigurator().apply(new Pigeon2Configuration()); m_imu.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); - yawVelocity.setUpdateFrequency(100.0); + m_yawSignal.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); + m_yawVelocitySignal.setUpdateFrequency(100.0); m_imu.optimizeBusUtilization(); m_yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_imu, m_imu.getYaw()); m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); @@ -28,9 +28,10 @@ public ImuIOPigeon2() { @Override public void updateInputs(ImuIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.connected = + BaseStatusSignal.refreshAll(m_yawSignal, m_yawVelocitySignal).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(m_yawSignal.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(m_yawVelocitySignal.getValueAsDouble()); inputs.odometryYawPositions = m_yawPositionQueue.stream() diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index d069ca1..76f1950 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.PoseEstimation; @@ -26,6 +27,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -39,6 +41,7 @@ public class Vision { private final boolean kTrustVisionXY = true; private final boolean kTrustVisionTheta = true; private final boolean kIgnoreVisionInSim = true; + private final boolean kIgnoreVisionInAuto = false; // #endregion private final VisionIO[] io; private final VisionIOInputs[] m_inputs; @@ -92,9 +95,19 @@ public void periodic() { m_newVisionUpdates = new ArrayList<>(); final List estimatedPosesToLog = new ArrayList<>(); - for (int i = 0; i < io.length; i++) { - for (PhotonPipelineResult result : m_inputs[i].results) { - Optional poseOptional = m_poseEstimators[i].update(result); + for (int ioIndex = 0; ioIndex < io.length; ioIndex++) { + for (int resultIndex = 0; resultIndex < m_inputs[ioIndex].results.length; resultIndex++) { + final byte[] rawResult = m_inputs[ioIndex].results[resultIndex]; + final Packet dataPacket = new Packet(1); + dataPacket.setData(rawResult); + if (dataPacket.getSize() < 1) + DriverStation.reportError("Somehow photonvision data packet is empty", true); + final PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket); + final double timestampSeconds = + (m_inputs[ioIndex].timestampsMillis[resultIndex] / 1e6) + - (result.getLatencyMillis() / 1e3); + result.setTimestampSeconds(timestampSeconds); + final Optional poseOptional = m_poseEstimators[ioIndex].update(result); poseOptional.ifPresent( pose -> { Pose3d estimatedPose = pose.estimatedPose; @@ -142,7 +155,8 @@ public void periodic() { public Matrix getEstimationStdDevs( Pose2d estimatedPose, List targets) { - if (Constants.kIsSim && kIgnoreVisionInSim) + if ((Constants.kIsSim && kIgnoreVisionInSim) + || (DriverStation.isAutonomous() && kIgnoreVisionInAuto)) return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); int tagCount = 0; double totalDistance = 0; diff --git a/src/main/java/frc/robot/vision/VisionIO.java b/src/main/java/frc/robot/vision/VisionIO.java index 08c0f56..d5c6967 100644 --- a/src/main/java/frc/robot/vision/VisionIO.java +++ b/src/main/java/frc/robot/vision/VisionIO.java @@ -3,20 +3,21 @@ import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.inputs.LoggableInputs; -import org.photonvision.targeting.PhotonPipelineResult; public interface VisionIO { public static class VisionIOInputs implements LoggableInputs { public String cameraName = ""; public Transform3d robotToCamera = new Transform3d(); - public PhotonPipelineResult[] results = new PhotonPipelineResult[] {}; + public double[] timestampsMillis = new double[] {}; + public byte[][] results = new byte[][] {}; public boolean connected = false; @Override public void toLog(LogTable table) { table.put("CameraName", cameraName); table.put("RobotToCamera", robotToCamera); + table.put("Timestamps", timestampsMillis); table.put("ResultCount", results.length); for (int i = 0; i < results.length; i++) { table.put("Result/" + Integer.toString(i), results[i]); @@ -28,10 +29,11 @@ public void toLog(LogTable table) { public void fromLog(LogTable table) { cameraName = table.get("CameraName", cameraName); robotToCamera = table.get("RobotToCamera", robotToCamera); + timestampsMillis = table.get("Timestamps", new double[] {0.0}); int resultCount = table.get("ResultCount", 0); - results = new PhotonPipelineResult[resultCount]; + results = new byte[resultCount][]; for (int i = 0; i < resultCount; i++) { - results[i] = table.get("Result/" + Integer.toString(i), new PhotonPipelineResult()); + results[i] = table.get("Result/" + i, new byte[] {}); } connected = table.get("Connected", false); } diff --git a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java index d68e5e8..dac02ea 100644 --- a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java @@ -6,8 +6,6 @@ import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.TimestampedRaw; import edu.wpi.first.wpilibj.Timer; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.PhotonPipelineResult; public class VisionIOPhotonVision implements VisionIO { @@ -35,19 +33,11 @@ public void updateInputs(VisionIOInputs inputs) { inputs.cameraName = m_cameraName; inputs.robotToCamera = m_robotToCamera; TimestampedRaw[] dataQueue = m_photonDataSubscriber.readQueue(); - inputs.results = new PhotonPipelineResult[dataQueue.length]; + inputs.results = new byte[dataQueue.length][]; for (int index = 0; index < dataQueue.length; index++) { - Packet dataPacket = new Packet(1); - dataPacket.setData(dataQueue[index].value); - if (dataPacket.getSize() < 1) { - throw new NullPointerException("Data packet is empty. This should NEVER happen."); - } - PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket); - double timestampSeconds = - (dataQueue[index].timestamp / 1e6) - (result.getLatencyMillis() / 1e3); - result.setTimestampSeconds(timestampSeconds); - inputs.results[index] = result; + inputs.results[index] = dataQueue[index].value; + inputs.timestampsMillis[index] = dataQueue[index].timestamp; } if (dataQueue.length > 0) {