diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c405902..e91c530 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,12 @@ package frc.robot; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -23,24 +28,21 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; -<<<<<<< Updated upstream -======= -import frc.robot.generated.TunerConstants; ->>>>>>> Stashed changes import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -<<<<<<< Updated upstream import frc.robot.subsystems.flywheel.Flywheel; import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; -======= ->>>>>>> Stashed changes -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import frc.robot.subsystems.vision.Vision; +import static frc.robot.subsystems.vision.VisionConstants.camera0Name; +import static frc.robot.subsystems.vision.VisionConstants.camera1Name; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOLimelight; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -50,33 +52,38 @@ */ public class RobotContainer { // Subsystems -<<<<<<< Updated upstream public static Drive drive; private final Flywheel flywheel; -======= - private final Drive drive; ->>>>>>> Stashed changes + private final Vision vision; // Controller private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; + private final LoggedDashboardNumber flywheelSpeedInput = + new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - switch (Constants.currentMode) { + switch (physicalConstants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations drive = new Drive( -<<<<<<< Updated upstream new GyroIOPigeon2(false), new ModuleIOTalonFX(0), new ModuleIOTalonFX(1), new ModuleIOTalonFX(2), new ModuleIOTalonFX(3)); + flywheel = new Flywheel(new FlywheelIOSparkMax()); + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIOLimelight(camera0Name, drive::getRotation), + new VisionIOLimelight(camera1Name, drive::getRotation)); + // drive = new Drive( // new GyroIOPigeon2(true), // new ModuleIOTalonFX(0), @@ -84,13 +91,6 @@ public RobotContainer() { // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); // flywheel = new Flywheel(new FlywheelIOTalonFX()); -======= - new GyroIOPigeon2(), - new ModuleIOTalonFX(TunerConstants.FrontLeft), - new ModuleIOTalonFX(TunerConstants.FrontRight), - new ModuleIOTalonFX(TunerConstants.BackLeft), - new ModuleIOTalonFX(TunerConstants.BackRight)); ->>>>>>> Stashed changes break; case SIM: @@ -98,18 +98,14 @@ public RobotContainer() { drive = new Drive( new GyroIO() {}, -<<<<<<< Updated upstream new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + flywheel = new Flywheel(new FlywheelIOSim()); -======= - new ModuleIOSim(TunerConstants.FrontLeft), - new ModuleIOSim(TunerConstants.FrontRight), - new ModuleIOSim(TunerConstants.BackLeft), - new ModuleIOSim(TunerConstants.BackRight)); ->>>>>>> Stashed changes + break; default: @@ -121,21 +117,20 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); -<<<<<<< Updated upstream flywheel = new Flywheel(new FlywheelIO() {}); -======= ->>>>>>> Stashed changes + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); break; } // Set up auto routines + NamedCommands.registerCommand( + "Run Flywheel", + Commands.startEnd( + () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) + .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up SysId routines - autoChooser.addOption( - "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); - autoChooser.addOption( - "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); autoChooser.addOption( "Drive SysId (Quasistatic Forward)", drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); @@ -146,6 +141,16 @@ public RobotContainer() { "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); autoChooser.addOption( "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Flywheel SysId (Quasistatic Forward)", + flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Quasistatic Reverse)", + flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Forward)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings configureButtonBindings(); @@ -158,28 +163,13 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // Default command, normal field-relative drive drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - - // Lock to 0° when A button is held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> new Rotation2d())); - - // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - - // Reset gyro to 0° when B button is pressed controller .b() .onTrue( @@ -189,14 +179,11 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); -<<<<<<< Updated upstream controller .a() .whileTrue( Commands.startEnd( () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); -======= ->>>>>>> Stashed changes } /** diff --git a/src/main/java/frc/robot/physicalConstants.java b/src/main/java/frc/robot/physicalConstants.java index 072bcf7..dbaeddf 100644 --- a/src/main/java/frc/robot/physicalConstants.java +++ b/src/main/java/frc/robot/physicalConstants.java @@ -45,8 +45,7 @@ public static class IntakeConstants { public static final boolean CURRENT_LIMIT_ENABLED = true; } - public static final class shooterConstants { - } + public static final class shooterConstants {} public static class ElevatorConstants { public static final double CURRENT_LIMIT = 40.0; @@ -70,7 +69,7 @@ public static final class PivotConstants { public static final double REDUCTION = (15.0 / 1.0) * (34.0 / 24.0) * (24.0 / 18.0) * (50.0 / 14.0); public static final double STOW_SETPOINT_DEG = 50.7; - + public static final double PIVOT_ZERO_ANGLE = 59; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ae030f7..06699f2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,18 +15,14 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.CANBus; +import edu.wpi.first.math.Matrix; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.hal.FRCNetComm.tInstances; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -38,17 +34,14 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.util.CircularBuffer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.Constants.Mode; -import frc.robot.generated.TunerConstants; +import frc.robot.physicalConstants; +import frc.robot.subsystems.vision.VisionIO; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -56,43 +49,27 @@ import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - // TunerConstants doesn't include these constants, so they are declared locally - static final double ODOMETRY_FREQUENCY = - new CANBus(TunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; - public static final double DRIVE_BASE_RADIUS = - Math.max( - Math.max( - Math.hypot(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontRight.LocationY), - Math.hypot(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); - - // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; - private static final double ROBOT_MOI = 6.883; - private static final double WHEEL_COF = 1.2; - private static final RobotConfig PP_CONFIG = - new RobotConfig( - ROBOT_MASS_KG, - ROBOT_MOI, - new ModuleConfig( - TunerConstants.FrontLeft.WheelRadius, - TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), - WHEEL_COF, - DCMotor.getKrakenX60Foc(1) - .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), - TunerConstants.FrontLeft.SlipCurrent, - 1), - getModuleTranslations()); + private static final double MAX_LINEAR_SPEED = + physicalConstants.SwerveConstants.MAX_LINEAR_SPEED * 0.85; + private static final double TRACK_WIDTH_X = + physicalConstants.SwerveConstants.TRACK_WIDTH_X_METERS; + private static final double TRACK_WIDTH_Y = + physicalConstants.SwerveConstants.TRACK_WIDTH_Y_METERS; + private static final double DRIVE_BASE_RADIUS = + physicalConstants.SwerveConstants.DRIVE_BASE_RADIUS; + private static final double MAX_ANGULAR_SPEED = + physicalConstants.SwerveConstants.MAX_ANGULAR_SPEED; + private static double multiplier = 1.0; + private static boolean toggle = false; - static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; - private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + public double yawVelocityRadPerSec = gyroInputs.yawVelocityRadPerSec; + + private final PIDController rotationController; + static final Lock odometryLock = new ReentrantLock(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -103,8 +80,26 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = + public SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + TimestampedT2d lastNoteLocT2d = new TimestampedT2d(new Translation2d(0, 0), -1.); + + public class TimestampedPose2d { + Pose2d pose; + double time; + } + + public class TimestampedT2d { + Translation2d translation; + double time; + + public TimestampedT2d(Translation2d translation, double time) { + this.translation = translation; + this.time = time; + } + } + + CircularBuffer robotPoseBuffer; public Drive( GyroIO gyroIO, @@ -113,27 +108,28 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0, TunerConstants.FrontLeft); - modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight); - modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft); - modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight); - - // Usage reporting for swerve template - HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); - // Start odometry thread + // Start threads (no-op for each if no signals have been created) PhoenixOdometryThread.getInstance().start(); - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( + AutoBuilder.configureHolonomic( this::getPose, this::setPose, - this::getChassisSpeeds, + () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + new HolonomicPathFollowerConfig( + new PIDConstants(5), + new PIDConstants(1.5), + physicalConstants.SwerveConstants.MAX_LINEAR_SPEED, + DRIVE_BASE_RADIUS, + new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( @@ -155,18 +151,32 @@ public Drive( null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + null, + this)); + + rotationController = new PIDController(0.1, 0, 0); + + rotationController.setTolerance(5); + rotationController.enableContinuousInput(-180, 180); + robotPoseBuffer = new CircularBuffer<>(11); } - @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); Logger.processInputs("Drive/Gyro", gyroInputs); for (var module : modules) { module.periodic(); } - odometryLock.unlock(); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -174,7 +184,6 @@ public void periodic() { module.stop(); } } - // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); @@ -212,9 +221,6 @@ public void periodic() { // Apply update poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } - - // Update gyro alert - gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); } /** @@ -224,28 +230,20 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - speeds.discretize(0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); - - // Log unoptimized setpoints and setpoint speeds - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(setpointStates[i]); + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); } - // Log optimized setpoints (runSetpoint mutates each state) - Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); - } - - /** Runs the drive in a straight line with the specified drive output. */ - public void runCharacterization(double output) { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(output); - } + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } /** Stops the drive. */ @@ -268,14 +266,12 @@ public void stopWithX() { /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)) - .withTimeout(1.0) - .andThen(sysId.quasistatic(direction)); + return sysId.quasistatic(direction); } /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + return sysId.dynamic(direction); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @@ -297,30 +293,6 @@ private SwerveModulePosition[] getModulePositions() { return states; } - /** Returns the measured chassis speeds of the robot. */ - @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); - } - - /** Returns the position of each module in radians. */ - public double[] getWheelRadiusCharacterizationPositions() { - double[] values = new double[4]; - for (int i = 0; i < 4; i++) { - values[i] = modules[i].getWheelRadiusCharacterizationPosition(); - } - return values; - } - - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ - public double getFFCharacterizationVelocity() { - double output = 0.0; - for (int i = 0; i < 4; i++) { - output += modules[i].getFFCharacterizationVelocity() / 4.0; - } - return output; - } - /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { @@ -337,7 +309,12 @@ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } - /** Adds a new timestamped vision measurement. */ + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -348,21 +325,21 @@ public void addVisionMeasurement( /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + return MAX_LINEAR_SPEED; } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; + return MAX_ANGULAR_SPEED; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), - new Translation2d(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), - new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..087dadf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,188 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +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.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, VisionIO... io) { + this.consumer = consumer; + this.io = io; + + // Initialize inputs + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + /** + * Returns the X angle to the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Initialize logging values + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + // Initialize logging values + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + // Add tag poses + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + // Loop over pose observations + for (var observation : inputs[cameraIndex].poseObservations) { + // Check whether to reject pose + boolean rejectPose = + observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) + > maxZError // Must have realistic Z coordinate + + // Must be within the field boundaries + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + // Add pose to log + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + // Skip if rejected + if (rejectPose) { + continue; + } + + // Calculate standard deviations + double stdDevFactor = + Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + // Send vision observation + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + // Log camera datadata + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[tagPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[robotPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + // Log summary data + Logger.recordOutput( + "Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", + allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", + allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); + } + + @FunctionalInterface + public static interface VisionConsumer { + public void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..7e7a34f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,58 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "front_camera"; + public static String camera1Name = "rear_camera"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..90648a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,49 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + public static class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + public static enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } + + public default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 07fe886..9f694ab 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,47 +1,155 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + package frc.robot.subsystems.vision; -import frc.robot.util.LimelightHelpers; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; +/** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; - String name; + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; - public VisionIOLimelight(String name, String name2) {} + /** + * Creates a new VisionIOLimelight. + * + * @param name The configured name of the Limelight. + * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. + */ + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } @Override public void updateInputs(VisionIOInputs inputs) { - inputs.mt2VisionPose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose; - inputs.mt1VisionPose = LimelightHelpers.getBotPose2d_wpiBlue(name); - inputs.timestampSeconds = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).timestampSeconds; - inputs.tagCount = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).tagCount; - inputs.tagSpan = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).tagSpan; - inputs.latency = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).latency; - inputs.avgTagDist = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).avgTagDist; - inputs.avgTagArea = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).avgTagArea; - - // -------------------------- - - inputs.iTX = LimelightHelpers.getTX(name); - inputs.iTY = LimelightHelpers.getTY(name); - inputs.iTA = LimelightHelpers.getTA(name); - inputs.iHB = LimelightHelpers.getLimelightNTTableEntry(name, "hb").getDouble(0.0); - inputs.iTV = LimelightHelpers.getTV(name); - inputs.iPIPELINELATENCY = LimelightHelpers.getLatency_Pipeline(name); - inputs.iCAPTURELATENCY = LimelightHelpers.getLatency_Capture(name); - inputs.iTHOR = LimelightHelpers.getLimelightNTTableEntry(name, "thor").getDouble(0.0); - inputs.iTVERT = LimelightHelpers.getLimelightNTTableEntry(name, "tvert").getDouble(0.0); - - // -------------------------- - - inputs.aTX = LimelightHelpers.getTX(name); - inputs.aTY = LimelightHelpers.getTY(name); - inputs.aTA = LimelightHelpers.getTA(name); - inputs.aHB = LimelightHelpers.getLimelightNTTableEntry(name, "hb").getDouble(0.0); - inputs.aTV = LimelightHelpers.getTV(name); - inputs.aPIPELINELATENCY = LimelightHelpers.getLatency_Pipeline(name); - inputs.aCAPTURELATENCY = LimelightHelpers.getLatency_Capture(name); - inputs.aTHOR = LimelightHelpers.getLimelightNTTableEntry(name, "thor").getDouble(0.0); - inputs.aTVERT = LimelightHelpers.getLimelightNTTableEntry(name, "tvert").getDouble(0.0); + // Update connection status based on whether an update has been seen in the last 250ms + inputs.connected = (RobotController.getFPGATime() - latencySubscriber.getLastChange()) < 250; + + // Update target observation + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 10; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag + rawSample.value.length >= 17 ? rawSample.value[16] : 0.0, + + // Tag count + (int) rawSample.value[8], + + // Average tag distance + rawSample.value[10], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 10; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[8], + + // Average tag distance + rawSample.value[10], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); } }