From eb5bb345b49388f3b1e616d01676e3f12c0b7d00 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Sun, 27 Oct 2024 11:41:11 -0500 Subject: [PATCH 01/24] switched to constructor instead of robotInit --- src/main/java/frc/robot/Robot.java | 17 +++++++++-------- .../robot/subsystems/drivetrain/DriveMap.java | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5e0ab9d..2debd66 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,16 +29,14 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; private Command m_autonomousCommand; - @Override - public void robotInit() { - // Start L2 logging - DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog()); - - // Initialize the robot container - m_robotContainer = new RobotContainer(isReal()); + public Robot() { + super(); + // Configure logging DataLogManager.start(); + DataLogManager.logConsoleOutput(true); + DataLogManager.logNetworkTables(true); + DriverStation.startDataLog(DataLogManager.getLog()); Epilogue.configure(config -> { if (isSimulation()) { // If running in simulation, then we'd want to re-throw any errors that @@ -53,6 +51,9 @@ public void robotInit() { // config.minimumImportance = Logged.Importance.CRITICAL; }); Epilogue.bind(this); + + // Initialize the robot container + m_robotContainer = new RobotContainer(isReal()); } @Override diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java b/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java index aa6c910..d69afdb 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java @@ -35,7 +35,7 @@ public class DriveMap { public static final String LimelightFrontName = "limelight-front"; public static final RobotConfig PathPlannerRobotConfiguration = new RobotConfig( Units.lbsToKilograms(120), - MomentOfInertia.ofBaseUnits(6, edu.wpi.first.units.Units.KilogramSquareMeters).baseUnitMagnitude(), // TODO??? + MomentOfInertia.ofBaseUnits(6, edu.wpi.first.units.Units.KilogramSquareMeters).baseUnitMagnitude(), // TODO: measure, maybe??? new ModuleConfig( Units.inchesToMeters(4), MaxSpeedMetersPerSecond, From b3310e3e28d494364418e26ed4e9b32dd1c0f91a Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Sun, 27 Oct 2024 11:41:36 -0500 Subject: [PATCH 02/24] simplified SwerveModuleIOSim for now. --- src/main/java/frc/robot/sim/MotorSim.java | 131 ------------------ .../swervemodule/SwerveModuleIOSim.java | 21 +-- 2 files changed, 3 insertions(+), 149 deletions(-) delete mode 100644 src/main/java/frc/robot/sim/MotorSim.java diff --git a/src/main/java/frc/robot/sim/MotorSim.java b/src/main/java/frc/robot/sim/MotorSim.java deleted file mode 100644 index 9e84817..0000000 --- a/src/main/java/frc/robot/sim/MotorSim.java +++ /dev/null @@ -1,131 +0,0 @@ -package frc.robot.sim; - -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import java.util.ArrayList; -import java.util.List; - -public class MotorSim implements MotorController, AutoCloseable { - - static final double _maxVoltage = 12; - static final double _period = 0.020; // 20ms - final double _maxVelocity; - double _direction = 1; - double _velocity = 0; - double _distance = 0; - List _followers = new ArrayList(); - - public void close() {} - - public MotorSim() { - _maxVelocity = 1; - } - - public MotorSim(double maxVelocity) { - _maxVelocity = maxVelocity; - } - - public MotorSim(double maxVelocity, double startingDistance) { - _maxVelocity = maxVelocity; - _distance = startingDistance; - } - - public void follow(MotorSim leader) { - leader.addFollower(this); - } - - public void addFollower(MotorSim follower) { - _followers.add(follower); - } - - /** - * Common interface for setting the speed of a motor controller. - * - * @param speed The speed to set. Value should be between -1.0 and 1.0. - */ - @Override - public void set(double speed) { - _velocity = speed * _maxVelocity * _direction; - - // also set any followers - for (MotorSim follower : _followers) { - follower.set(speed); - } - } - - /** - * Sets the voltage output of the MotorController. Compensates for the current bus voltage to - * ensure that the desired voltage is output even if the battery voltage is below 12V - highly - * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward - * calculation). - * - *

NOTE: This function *must* be called regularly in order for voltage compensation to work - * properly - unlike the ordinary set function, it is not "set it and forget it." - * - * @param outputVolts The voltage to output. - */ - @Override - public void setVoltage(double outputVolts) { - set(outputVolts / _maxVoltage); - } - - /** - * Common interface for getting the current set speed of a motor controller. - * - * @return The current set speed. Value is between -1.0 and 1.0. - */ - @Override - public double get() { - return _velocity / _maxVelocity; - } - - /** - * Common interface for inverting direction of a motor controller. - * - * @param isInverted The state of inversion true is inverted. - */ - @Override - public void setInverted(boolean isInverted) { - _direction = isInverted ? -1 : 1; - } - - /** - * Common interface for returning if a motor controller is in the inverted state or not. - * - * @return isInverted The state of the inversion true is inverted. - */ - @Override - public boolean getInverted() { - return _direction < 0; - } - - /** Disable the motor controller. */ - @Override - public void disable() { - stopMotor(); - } - - /** - * Stops motor movement. Motor can be moved again by calling set without having to re-enable the - * motor. - */ - @Override - public void stopMotor() { - _velocity = 0; - } - - public void setDistance(double distance) { - _distance = distance; - } - - public double getDistance() { - return _distance; - } - - public double getVelocity() { - return _velocity; - } - - public void periodic() { - _distance += _velocity * _period; - } -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index 69f4980..3bcba7c 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -1,40 +1,25 @@ package frc.robot.subsystems.drivetrain.swervemodule; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.sim.MotorSim; -import frc.robot.subsystems.drivetrain.DriveMap; - public class SwerveModuleIOSim implements ISwerveModuleIO { private SwerveModuleIOInputs m_inputs; - // Devices - private MotorSim m_driveMotor; - private double _steerAngle = 0; - public SwerveModuleIOSim(SwerveModuleConfig moduleMap) { - m_driveMotor = new MotorSim(DriveMap.MaxSpeedMetersPerSecond); } @Override public SwerveModuleIOInputs getInputs() { - m_inputs.ModuleState.angle = Rotation2d.fromDegrees(_steerAngle); - m_inputs.ModuleState.speedMetersPerSecond = m_driveMotor.getVelocity(); - m_inputs.ModulePosition.angle = Rotation2d.fromDegrees(_steerAngle); - m_inputs.ModulePosition.distanceMeters = m_driveMotor.getDistance(); - return m_inputs; } @Override public void setOutputs(SwerveModuleIOOutputs outputs) { - _steerAngle = outputs.DesiredState.angle.getDegrees(); - var normalizedSpeed = outputs.DesiredState.speedMetersPerSecond / DriveMap.MaxSpeedMetersPerSecond; - m_driveMotor.set(normalizedSpeed); + // Right now, just assume that the module is perfect and has no losses. + // This may be replaced with a more realistic simulation later. + m_inputs.ModuleState = outputs.DesiredState; } @Override public void stopMotors() { - m_driveMotor.stopMotor(); } } From 9a78f42f74e45eebc171e56287d209b6d5171523 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Sun, 27 Oct 2024 11:41:41 -0500 Subject: [PATCH 03/24] fleshed out DrivetrainSim --- .../drivetrain/DrivetrainIOReal.java | 2 + .../drivetrain/DrivetrainIOSim.java | 198 ++++++++++++++++-- .../drivetrain/DrivetrainSubsystem.java | 4 +- 3 files changed, 191 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java index ff88432..c8c6ffb 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java @@ -33,6 +33,7 @@ public class DrivetrainIOReal implements IDrivetrainIO { private SwerveDrivePoseEstimator m_poseEstimator; private ISwerveModuleIO m_frontLeftModule, m_frontRightModule, m_rearLeftModule, m_rearRightModule; + @Logged(name = "FrontLeftInputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOInputs m_frontLeftInputs; @Logged(name = "FrontRightInputs", importance = Logged.Importance.CRITICAL) @@ -41,6 +42,7 @@ public class DrivetrainIOReal implements IDrivetrainIO { private SwerveModuleIOInputs m_rearLeftInputs; @Logged(name = "RearRightInputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOInputs m_rearRightInputs; + @Logged(name = "FrontLeftOutputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOOutputs m_frontLeftOutputs; @Logged(name = "FrontRightOutputs", importance = Logged.Importance.CRITICAL) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index b3f8e72..5bf88b4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -1,45 +1,219 @@ package frc.robot.subsystems.drivetrain; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +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.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import frc.robot.Robot; +import frc.robot.subsystems.drivetrain.swervemodule.ISwerveModuleIO; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOInputs; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOSim; public class DrivetrainIOSim implements IDrivetrainIO { + private DrivetrainIOInputs m_inputs; + + private AnalogGyroSim m_gyroSim; + private ISwerveModuleIO m_frontLeftModule, m_frontRightModule, m_rearLeftModule, m_rearRightModule; + + private SwerveDriveKinematics m_kinematics; + private SwerveDrivePoseEstimator m_poseEstimator; + + @Logged(name = "FrontLeftInputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOInputs m_frontLeftInputs; + @Logged(name = "FrontRightInputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOInputs m_frontRightInputs; + @Logged(name = "RearLeftInputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOInputs m_rearLeftInputs; + @Logged(name = "RearRightInputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOInputs m_rearRightInputs; + + @Logged(name = "FrontLeftOutputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOOutputs m_frontLeftOutputs; + @Logged(name = "FrontRightOutputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOOutputs m_frontRightOutputs; + @Logged(name = "RearLeftOutputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOOutputs m_rearLeftOutputs; + @Logged(name = "RearRightOutputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOOutputs m_rearRightOutputs; + + public DrivetrainIOSim() { + m_inputs = new DrivetrainIOInputs(); + + m_gyroSim = new AnalogGyroSim(new AnalogGyro(0)); + + // Create swerve modules in CCW order from FL to FR + m_frontLeftModule = new SwerveModuleIOSim(DriveMap.FrontLeftSwerveModule); + m_frontRightModule = new SwerveModuleIOSim(DriveMap.FrontRightSwerveModule); + m_rearLeftModule = new SwerveModuleIOSim(DriveMap.RearLeftSwerveModule); + m_rearRightModule = new SwerveModuleIOSim(DriveMap.RearRightSwerveModule); + + // Create kinematics in order FL, FR, RL, RR + m_kinematics = + new SwerveDriveKinematics( + DriveMap.FrontLeftSwerveModule.ModuleLocation, + DriveMap.FrontRightSwerveModule.ModuleLocation, + DriveMap.RearLeftSwerveModule.ModuleLocation, + DriveMap.RearRightSwerveModule.ModuleLocation + ); + + // Create pose estimator + m_poseEstimator = + new SwerveDrivePoseEstimator(m_kinematics, Rotation2d.fromDegrees(m_gyroSim.getAngle()), + getModulePositions(), new Pose2d()); + } + @Override public DrivetrainIOInputs getInputs() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getInputs'"); + m_frontLeftInputs = m_frontLeftModule.getInputs(); + m_frontRightInputs = m_frontRightModule.getInputs(); + m_rearLeftInputs = m_rearLeftModule.getInputs(); + m_rearRightInputs = m_rearRightModule.getInputs(); + + m_inputs.GyroAngle = Rotation2d.fromDegrees(m_gyroSim.getAngle()); + // m_inputs.GyroAccelX = m_gyro.getAccelerationX().getValueAsDouble(); + // m_inputs.GyroAccelY = m_gyro.getAccelerationY().getValueAsDouble(); + // m_inputs.GyroAccelZ = m_gyro.getAccelerationZ().getValueAsDouble(); + m_inputs.RobotRelativeChassisSpeeds = m_kinematics.toChassisSpeeds(getModuleStates()); + var modulePositions = getModulePositions(); + m_inputs.EstimatedRobotPose = m_poseEstimator.update(m_inputs.GyroAngle, modulePositions); + + return m_inputs; } @Override public void setOutputs(DrivetrainIOOutputs outputs) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setOutputs'"); + if (outputs.SnapEnabled) { + // Set GyroSim to the snap angle with no simulated delays. + m_gyroSim.setAngle(outputs.SnapSetpoint.getDegrees()); + } + + switch (outputs.ControlMode) { + case kRobotRelative: + driveRobotRelative(outputs.DesiredChassisSpeeds, outputs.SnapEnabled); + break; + case kFieldRelative: + drivePathPlanner(outputs.DesiredChassisSpeeds); + break; + default: + break; + } + + m_frontLeftModule.setOutputs(m_frontLeftOutputs); + m_frontRightModule.setOutputs(m_frontRightOutputs); + m_rearLeftModule.setOutputs(m_rearLeftOutputs); + m_rearRightModule.setOutputs(m_rearRightOutputs); } @Override public void resetGyro() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'resetGyro'"); + m_gyroSim.setAngle(Robot.onBlueAlliance() ? 180 : 0); + + m_poseEstimator.resetPosition(Rotation2d.fromDegrees(m_gyroSim.getAngle()), + getModulePositions(), m_poseEstimator.getEstimatedPosition()); } @Override public void setEstimatorPose(Pose2d pose) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setEstimatorPose'"); + m_poseEstimator.resetPosition(Rotation2d.fromDegrees(m_gyroSim.getAngle()), getModulePositions(), pose); } @Override public void addPoseEstimatorVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDeviations) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'addPoseEstimatorVisionMeasurement'"); + m_poseEstimator.addVisionMeasurement(pose, timestamp, stdDeviations); } @Override public void stopAllMotors() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'addPoseEstimatorVisionMeasurement'"); + m_frontLeftModule.stopMotors(); + m_frontRightModule.stopMotors(); + m_rearLeftModule.stopMotors(); + m_rearRightModule.stopMotors(); + } + + /** + * Drives robot-relative using a ChassisSpeeds + * @param desiredChassisSpeeds The desired speeds of the robot + */ + private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snapAngleEnabled) { + // If snap-to is enabled, calculate and override the input rotational speed to reach the setpoint + if (snapAngleEnabled) { + // Report back that snap is on-target since we're assuming there is no loss in the simulation + m_inputs.SnapOnTarget = true; + } + + // Correct drift by taking the input speeds and converting them to a desired per-period speed. This is known as "discretizing" + desiredChassisSpeeds = ChassisSpeeds.discretize(desiredChassisSpeeds, 0.02); + + // Calculate the module states from the chassis speeds + var swerveModuleStates = m_kinematics.toSwerveModuleStates(desiredChassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveMap.MaxSpeedMetersPerSecond); + + // Set the desired states for each module + setDesiredModuleStates(swerveModuleStates); + } + + /** + * Facilitates driving using PathPlanner generated speeds + * @param robotRelativeSpeeds + */ + private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { + if (Robot.onRedAlliance()) { + // If we're on the red alliance, we need to flip the gyro + var gyroAngle = Rotation2d.fromDegrees(m_gyroSim.getAngle()).plus(Rotation2d.fromDegrees(180)); + + // Convert the robot-relative speeds to field-relative speeds with the flipped gyro + var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeSpeeds, gyroAngle); + + // Convert back to robot-relative speeds, also with the flipped gyro + driveRobotRelative(ChassisSpeeds.fromFieldRelativeSpeeds(fieldSpeeds, gyroAngle), false); + } else { + driveRobotRelative(robotRelativeSpeeds, false); + } + } + + /** + * Sets the desired states for each swerve module in order FL, FR, RL, RR + * @param desiredStates + */ + private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { + m_frontLeftOutputs.DesiredState = desiredStates[0]; + m_frontRightOutputs.DesiredState = desiredStates[1]; + m_rearLeftOutputs.DesiredState = desiredStates[2]; + m_rearRightOutputs.DesiredState = desiredStates[3]; + } + + /** + * Gets the instantaneous states for each swerve module in FL, FR, RL, RR order + */ + private SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + m_frontLeftInputs.ModuleState, + m_frontRightInputs.ModuleState, + m_rearLeftInputs.ModuleState, + m_rearRightInputs.ModuleState, + }; + } + + /** + * Gets the cumulative positions for each swerve module in FL, FR, RL, RR order + */ + private SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + m_frontLeftInputs.ModulePosition, + m_frontRightInputs.ModulePosition, + m_rearLeftInputs.ModulePosition, + m_rearRightInputs.ModulePosition, + }; } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 8a34f71..84ba385 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -77,7 +77,9 @@ public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverD m_driverDashboard = driverDashboard; // Create IO - m_driveio = isReal ? new DrivetrainIOReal() : new DrivetrainIOSim(); + m_driveio = isReal + ? new DrivetrainIOReal() + : new DrivetrainIOSim(); m_inputs = m_driveio.getInputs(); m_outputs = new DrivetrainIOOutputs(); From 81a76531a62a8d3d3950abc8cfccd8bed8227a76 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Mon, 28 Oct 2024 16:57:23 -0500 Subject: [PATCH 04/24] rewrote vision into single subsystem, with struct logging for custom LL pose and inputs --- src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 9 +- .../java/frc/robot/subsystems/Limelight.java | 298 ------------------ .../drivetrain/DrivetrainSubsystem.java | 82 ++--- .../robot/subsystems/vision/LimeLightNT.java | 294 +++++++++++++++++ .../subsystems/vision/VisionSubsystem.java | 122 +++++++ .../java/prime/physics/LimelightPose.java | 46 ++- .../prime/physics/LimelightPoseStruct.java | 62 ++++ .../java/prime/vision/LimelightInputs.java | 96 ++++++ .../prime/vision/LimelightInputsStruct.java | 86 +++++ 10 files changed, 751 insertions(+), 352 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/Limelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision/LimeLightNT.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java create mode 100644 src/main/java/prime/physics/LimelightPoseStruct.java create mode 100644 src/main/java/prime/vision/LimelightInputs.java create mode 100644 src/main/java/prime/vision/LimelightInputsStruct.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2debd66..7d61eda 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,8 +97,8 @@ public void autonomousInit() { m_robotContainer.Drivetrain.resetGyro(); } else { // Schedule the auto command - m_robotContainer.Drivetrain.EnableContinuousPoseEstimationFront = true; - m_robotContainer.Drivetrain.EnableContinuousPoseEstimationRear = true; + m_robotContainer.Drivetrain.EstimatePoseUsingFrontCamera = true; + m_robotContainer.Drivetrain.EstimatePoseUsingRearCamera = true; if (onRedAlliance()) m_robotContainer.Drivetrain.resetGyro(); @@ -126,8 +126,8 @@ public void teleopInit() { new ChasePattern(onRedAlliance() ? Color.RED : Color.BLUE, 0.5, false) ); - m_robotContainer.Drivetrain.EnableContinuousPoseEstimationFront = false; - m_robotContainer.Drivetrain.EnableContinuousPoseEstimationRear = false; + m_robotContainer.Drivetrain.EstimatePoseUsingFrontCamera = false; + m_robotContainer.Drivetrain.EstimatePoseUsingRearCamera = false; } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d673af..def9679 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,8 @@ import frc.robot.subsystems.*; import frc.robot.subsystems.drivetrain.DriveMap; import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; + import java.util.Map; import prime.control.Controls; import prime.control.HolonomicControlStyle; @@ -35,6 +37,7 @@ public class RobotContainer { private PrimeXboxController m_driverController; private PrimeXboxController m_operatorController; + public VisionSubsystem Vision; @Logged(name = "Drivetrain", importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; public Shooter Shooter; @@ -54,7 +57,11 @@ public RobotContainer(boolean isReal) { // Create new subsystems LEDs = new PwmLEDs(); DriverDashboard = new DriverDashboard(); - Drivetrain = new DrivetrainSubsystem(isReal, LEDs, DriverDashboard); + Vision = new VisionSubsystem(new String[] { + DriveMap.LimelightFrontName, + DriveMap.LimelightRearName + }); + Drivetrain = new DrivetrainSubsystem(isReal, LEDs, DriverDashboard, Vision); Shooter = new Shooter(LEDs); Intake = new Intake(); Climbers = new Climbers(DriverDashboard); diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java deleted file mode 100644 index e3c940a..0000000 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ /dev/null @@ -1,298 +0,0 @@ -// wip - -package frc.robot.subsystems; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -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.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; -import prime.physics.LimelightPose; - -public class Limelight extends SubsystemBase implements AutoCloseable { - - private NetworkTable m_limelightTable; - private ExecutorService m_executorService = Executors.newSingleThreadExecutor(); - - /** - * Creates a new Limelight subsystem and sets the camera's pose in the coordinate system of the robot. - * @param cameraPose - */ - public Limelight(String tableName) { - m_limelightTable = NetworkTableInstance.getDefault().getTable(tableName); - } - - //#region Basic Targeting Data - - /** - * Returns Horizontal Offset From Crosshair To Target (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) - */ - public Rotation2d getHorizontalOffsetFromTarget() { - return Rotation2d.fromDegrees(m_limelightTable.getEntry("tx").getDouble(0.0)); - } - - /** - * Returns Vertical Offset From Crosshair To Target (LL1: -20.5 degrees to 20.5 degrees / LL2: -24.85 to 24.85 degrees) - */ - public Rotation2d getVerticalOffsetFromTarget() { - return Rotation2d.fromDegrees(m_limelightTable.getEntry("ty").getDouble(0.0)); - } - - /** - * Returns Target Area (0% of image to 100% of image) - */ - public double getTargetArea() { - return m_limelightTable.getEntry("ta").getDouble(0.0); - } - - /** - * The pipeline's latency contribution (ms). Add to "cl" to get total latency. - */ - public long getPipelineLatencyMs() { - return (long) m_limelightTable.getEntry("tl").getDouble(0.0); - } - - /** - * Capture pipeline latency (ms). Time between the end of the exposure of the middle row of the sensor to the beginning of the tracking pipeline. - */ - public long getCapturePipelineLatencyMs() { - return (long) m_limelightTable.getEntry("cl").getDouble(0.0); - } - - /** - * The total latency of the capture and pipeline processing in milliseconds. - * @return - */ - public long getTotalLatencyMs() { - return getPipelineLatencyMs() + getCapturePipelineLatencyMs(); - } - - //#endregion - - //#region AprilTag and 3D Data - - /** - * ID of the primary in-view AprilTag - */ - public int getApriltagId() { - return (int) m_limelightTable.getEntry("tid").getDouble(-1); - } - - /** - * Returns the number of AprilTags in the image. - * @return - */ - public double getTagCount() { - // Robot transform in field-space. Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency (cl+tl), tag count, tag span, average tag distance from camera, average tag area (percentage of image) - var botPose = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); - - return botPose[7]; - } - - /** - * Robot transform in field-space. - */ - public LimelightPose getRobotPose() { - var poseData = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * Robot transform in field-space (alliance driverstation WPILIB origin). - * @param alliance - */ - public LimelightPose getRobotPose(DriverStation.Alliance alliance) { - var poseData = alliance == Alliance.Blue - ? m_limelightTable.getEntry("botpose_wpiblue").getDoubleArray(new double[11]) - : m_limelightTable.getEntry("botpose_wpired").getDoubleArray(new double[11]); - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the robot in the coordinate system of the primary in-view AprilTag - */ - public LimelightPose getRobotPoseInTargetSpace() { - var poseData = m_limelightTable.getEntry("botpose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the camera in the coordinate system of the primary in-view AprilTag - */ - public LimelightPose getCameraPoseInTargetSpace() { - var poseData = m_limelightTable.getEntry("camerapose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the camera in the coordinate system of the robot - */ - public LimelightPose getCameraPoseInRobotSpace() { - var poseData = m_limelightTable.getEntry("camerapose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera - */ - public LimelightPose getTargetPoseInCameraSpace() { - var poseData = m_limelightTable.getEntry("targetpose_cameraspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot - */ - public LimelightPose getTargetPoseInRobotSpace() { - var poseData = m_limelightTable.getEntry("targetpose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * Calculates a trust value based on the number of tags in view. - * @return - */ - public Matrix calculateTrust(double tagCount) { - // Trust level is a function of the number of tags in view - // var trustLevel = 0.490956d + Math.pow(9998.51d, -(6.95795d * tagCount)); - - var trustLevel = tagCount >= 2.0 ? 2 : 20; - - // X Y Z trust levels (never trust Z) - return VecBuilder.fill(trustLevel, trustLevel, 999999); - } - - //#endregion - - //#region Camera Controls - - /** - * Sets limelight’s LED state. - * 0 = use the LED Mode set in the current pipeline. - * 1 = force off. - * 2 = force blink. - * 3 = force on. - * @param mode - */ - public void setLedMode(int mode) { - m_limelightTable.getEntry("ledMode").setNumber(mode); - } - - /** - * Forces the LED to blink a specified number of times, then returns to pipeline control. - */ - public void blinkLed(int blinkCount) { - m_executorService.submit(() -> { - // Blink the LED X times with 100ms on, 200ms off for each blink - for (int i = 0; i < blinkCount; i++) { - m_limelightTable.getEntry("ledMode").setNumber(3); - - try { - Thread.sleep(100); - } catch (Exception e) { - Thread.currentThread().interrupt(); - } - - m_limelightTable.getEntry("ledMode").setNumber(1); - try { - Thread.sleep(200); - } catch (Exception e) { - Thread.currentThread().interrupt(); - } - } - - // Then return to pipeline control - setLedMode(0); - }); - } - - /** - * Sets limelight’s operation mode. - * 0 = Vision processor. - * 1 = Driver Camera (Increases exposure, disables vision processing). - * @param mode - */ - public void setCameraMode(int mode) { - m_limelightTable.getEntry("camMode").setNumber(mode); - } - - /** - * Sets limelight’s pipeline. - * @param pipeline - */ - public void setPipeline(int pipeline) { - m_limelightTable.getEntry("pipeline").setNumber(pipeline); - } - - /** - * Side-by-side streams (Note: USB output stream is not affected by this mode) - * @param mode - */ - public void setPiPStreamingMode(int mode) { - m_limelightTable.getEntry("stream").setNumber(mode); - } - - /** - * Allows users to take snapshots during a match - * @param mode - */ - public void takeSnapshot() { - m_limelightTable.getEntry("snapshot").setNumber(0); // Reset - m_limelightTable.getEntry("snapshot").setNumber(1); // Take snapshot - m_limelightTable.getEntry("snapshot").setNumber(0); // Reset - } - - /** - * Set the camera's pose in the coordinate system of the robot. - * @param pose - */ - public void setCameraPose(Pose3d pose) { - var poseData = new double[] { - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getTranslation().getZ(), - Units.radiansToDegrees(pose.getRotation().getX()), - Units.radiansToDegrees(pose.getRotation().getY()), - Units.radiansToDegrees(pose.getRotation().getZ()), - }; - - m_limelightTable.getEntry("camerapose_robotspace_set").setDoubleArray(poseData); - } - - //#endregion - - public void periodic() { - // Level2 logging - SmartDashboard.putNumber("Limelight/PrimaryTargetID", getApriltagId()); - SmartDashboard.putNumber("Limelight/HorizontalOffset", getHorizontalOffsetFromTarget().getDegrees()); - } - - public boolean isSpeakerCenterTarget(int apriltagId) { - return apriltagId == 4 || apriltagId == 7; - } - - public boolean isValidApriltag(int apriltagId) { - return apriltagId >= 1 && apriltagId <= 16; - } - - public void close() { - m_executorService.shutdown(); - } -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 84ba385..8cc78ba 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -23,8 +22,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.subsystems.DriverDashboard; -import frc.robot.subsystems.Limelight; import frc.robot.subsystems.PwmLEDs; +import frc.robot.subsystems.vision.VisionSubsystem; + import java.util.Map; import prime.control.SwerveControlSuppliers; import prime.control.LEDs.Color; @@ -61,17 +61,16 @@ public class DrivetrainSubsystem extends SubsystemBase { private DrivetrainIOOutputs m_outputs; // Vision, Kinematics, odometry - public Limelight LimelightRear; - public Limelight LimelightFront; - @Logged(name = "FrontPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) - public boolean EnableContinuousPoseEstimationFront = true; - @Logged(name = "RearPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) - public boolean EnableContinuousPoseEstimationRear = true; + public VisionSubsystem m_vision; + @Logged(name = "FrontVisionPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) + public boolean EstimatePoseUsingFrontCamera = true; + @Logged(name = "RearVisionPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) + public boolean EstimatePoseUsingRearCamera = true; /** * Creates a new Drivetrain. */ - public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverDashboard) { + public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverDashboard, VisionSubsystem vision) { setName("Drivetrain"); m_leds = leds; m_driverDashboard = driverDashboard; @@ -83,9 +82,12 @@ public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverD m_inputs = m_driveio.getInputs(); m_outputs = new DrivetrainIOOutputs(); - LimelightRear = new Limelight(DriveMap.LimelightRearName); - LimelightFront = new Limelight(DriveMap.LimelightFrontName); + m_vision = vision; + + configurePathPlanner(); + } + private void configurePathPlanner() { // ==================================== PATHPLANNER 2025 ==================================== // Load the RobotConfig from the GUI settings, or use the default if an exception occurs RobotConfig config = DriveMap.PathPlannerRobotConfiguration; @@ -97,11 +99,11 @@ public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverD } // Set up PP to feed current path poses to the field widget - PathPlannerLogging.setLogCurrentPoseCallback(pose -> driverDashboard.FieldWidget.setRobotPose(pose)); + PathPlannerLogging.setLogCurrentPoseCallback(pose -> m_driverDashboard.FieldWidget.setRobotPose(pose)); PathPlannerLogging.setLogTargetPoseCallback(pose -> - driverDashboard.FieldWidget.getObject("target pose").setPose(pose) + m_driverDashboard.FieldWidget.getObject("target pose").setPose(pose) ); - PathPlannerLogging.setLogActivePathCallback(poses -> driverDashboard.FieldWidget.getObject("path").setPoses(poses)); + PathPlannerLogging.setLogActivePathCallback(poses -> m_driverDashboard.FieldWidget.getObject("path").setPoses(poses)); // Configure PathPlanner holonomic control AutoBuilder.configure( @@ -207,37 +209,37 @@ private void evaluatePoseEstimation() { currentSpeeds.vyMetersPerSecond < 2; SmartDashboard.putBoolean("Drive/PoseEstimation/WithinTrustedVelocity", withinTrustedVelocity); - EnableContinuousPoseEstimationRear = m_driverDashboard.RearPoseEstimationSwitch.getBoolean(false); - SmartDashboard.putBoolean("Drive/PoseEstimation/RearEstimationEnabled", EnableContinuousPoseEstimationRear); - if (EnableContinuousPoseEstimationRear) { - // Rear Limelight + EstimatePoseUsingFrontCamera = m_driverDashboard.FrontPoseEstimationSwitch.getBoolean(false); + SmartDashboard.putBoolean("Drive/PoseEstimation/FrontEstimationEnabled", EstimatePoseUsingFrontCamera); + if (EstimatePoseUsingFrontCamera) { + // Front Limelight // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator - var primaryTarget = LimelightRear.getApriltagId(); - var isValidTarget = LimelightRear.isValidApriltag(primaryTarget); - SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isValidTarget); - - m_driverDashboard.RearApTagIdField.setDouble(primaryTarget); - m_driverDashboard.RearApTagOffsetDial.setDouble(LimelightRear.getHorizontalOffsetFromTarget().getDegrees()); + var frontInputs = m_vision.getLimelightInputs(0); + var frontIsValidTarget = m_vision.isValidApriltag(frontInputs.ApriltagId); + m_driverDashboard.FrontApTagIdField.setDouble(frontInputs.ApriltagId); + SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", frontIsValidTarget); - if (isValidTarget && withinTrustedVelocity) { - var llPose = LimelightRear.getRobotPose(Alliance.Blue); + if (frontIsValidTarget && withinTrustedVelocity) { + var llPose = frontInputs.BlueAllianceOriginFieldSpaceRobotPose; m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); } } - EnableContinuousPoseEstimationFront = m_driverDashboard.FrontPoseEstimationSwitch.getBoolean(false); - SmartDashboard.putBoolean("Drive/PoseEstimation/FrontEstimationEnabled", EnableContinuousPoseEstimationFront); - if (EnableContinuousPoseEstimationFront) { - // Front Limelight + EstimatePoseUsingRearCamera = m_driverDashboard.RearPoseEstimationSwitch.getBoolean(false); + SmartDashboard.putBoolean("Drive/PoseEstimation/RearEstimationEnabled", EstimatePoseUsingRearCamera); + if (EstimatePoseUsingRearCamera) { + // Rear Limelight // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator - var frontPrimaryTarget = LimelightFront.getApriltagId(); - var frontIsValidTarget = LimelightFront.isValidApriltag(frontPrimaryTarget); - m_driverDashboard.FrontApTagIdField.setDouble(frontPrimaryTarget); - SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", frontIsValidTarget); + var rearInputs = m_vision.getLimelightInputs(1); + var isValidTarget = m_vision.isValidApriltag(rearInputs.ApriltagId); + SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isValidTarget); - if (frontIsValidTarget && withinTrustedVelocity) { - var llPose = LimelightFront.getRobotPose(Alliance.Blue); + m_driverDashboard.RearApTagIdField.setDouble(rearInputs.ApriltagId); + m_driverDashboard.RearApTagOffsetDial.setDouble(rearInputs.TargetHorizontalOffset.getDegrees()); + + if (isValidTarget && withinTrustedVelocity) { + var llPose = rearInputs.BlueAllianceOriginFieldSpaceRobotPose; m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); } @@ -335,12 +337,12 @@ public Command disableSnapToCommand() { */ public Command enableLockOn() { return Commands.run(() -> { - var targetedAprilTag = LimelightRear.getApriltagId(); + var rearLimelightInputs = m_vision.getLimelightInputs(1); - // If targetedAprilTag is in validTargets, snap to its offset - if (LimelightRear.isSpeakerCenterTarget(targetedAprilTag)) { + // If targeted AprilTag is in validTargets, snap to its offset + if (m_vision.isSpeakerCenterTarget(rearLimelightInputs.ApriltagId)) { // Calculate the target heading - var horizontalOffsetDeg = LimelightRear.getHorizontalOffsetFromTarget().getDegrees(); + var horizontalOffsetDeg = rearLimelightInputs.TargetHorizontalOffset.getDegrees(); var robotHeadingDeg = getHeadingDegrees(); var targetHeadingDeg = robotHeadingDeg - horizontalOffsetDeg; diff --git a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java new file mode 100644 index 0000000..655b8e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java @@ -0,0 +1,294 @@ +package frc.robot.subsystems.vision; + +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +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.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import prime.physics.LimelightPose; +import prime.vision.LimelightInputs; + +public class LimeLightNT implements AutoCloseable { + private NetworkTable m_limelightTable; + private ExecutorService m_executorService = Executors.newSingleThreadExecutor(); + + private LimelightInputs m_inputs; + + public LimeLightNT(NetworkTableInstance instance, String tableName) { + m_limelightTable = instance.getTable(tableName); + } + + public LimelightInputs getInputs() { + m_inputs.TargetHorizontalOffset = getHorizontalOffsetFromTarget(); + m_inputs.TargetVerticalOffset = getVerticalOffsetFromTarget(); + m_inputs.TargetArea = getTargetArea(); + m_inputs.PipelineLatencyMs = getPipelineLatencyMs(); + m_inputs.CapturePipelineLatencyMs = getCapturePipelineLatencyMs(); + m_inputs.TotalLatencyMs = getTotalLatencyMs(); + m_inputs.ApriltagId = getApriltagId(); + m_inputs.TagCount = getTagCount(); + m_inputs.FieldSpaceRobotPose = getRobotPose(); + m_inputs.RedAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Red); + m_inputs.BlueAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Blue); + + return m_inputs; + } + + //#region Basic Targeting Data + + /** + * Returns Horizontal Offset From Crosshair To Target (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) + */ + public Rotation2d getHorizontalOffsetFromTarget() { + return Rotation2d.fromDegrees(m_limelightTable.getEntry("tx").getDouble(0.0)); + } + + /** + * Returns Vertical Offset From Crosshair To Target (LL1: -20.5 degrees to 20.5 degrees / LL2: -24.85 to 24.85 degrees) + */ + public Rotation2d getVerticalOffsetFromTarget() { + return Rotation2d.fromDegrees(m_limelightTable.getEntry("ty").getDouble(0.0)); + } + + /** + * Returns Target Area (0% of image to 100% of image) + */ + public double getTargetArea() { + return m_limelightTable.getEntry("ta").getDouble(0.0); + } + + /** + * The pipeline's latency contribution (ms). Add to "cl" to get total latency. + */ + public long getPipelineLatencyMs() { + return (long) m_limelightTable.getEntry("tl").getDouble(0.0); + } + + /** + * Capture pipeline latency (ms). Time between the end of the exposure of the middle row of the sensor to the beginning of the tracking pipeline. + */ + public long getCapturePipelineLatencyMs() { + return (long) m_limelightTable.getEntry("cl").getDouble(0.0); + } + + /** + * The total latency of the capture and pipeline processing in milliseconds. + * @return + */ + public long getTotalLatencyMs() { + return getPipelineLatencyMs() + getCapturePipelineLatencyMs(); + } + + //#endregion + + //#region AprilTag and 3D Data + + /** + * ID of the primary in-view AprilTag + */ + public int getApriltagId() { + return (int) m_limelightTable.getEntry("tid").getDouble(-1); + } + + /** + * Returns the number of AprilTags in the image. + * @return + */ + public double getTagCount() { + // Robot transform in field-space. Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency (cl+tl), tag count, tag span, average tag distance from camera, average tag area (percentage of image) + var botPose = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); + + return botPose[7]; + } + + /** + * Robot transform in field-space. + */ + public LimelightPose getRobotPose() { + var poseData = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * Robot transform in field-space (alliance driverstation WPILIB origin). + * @param alliance + */ + public LimelightPose getRobotPose(DriverStation.Alliance alliance) { + var poseData = alliance == Alliance.Blue + ? m_limelightTable.getEntry("botpose_wpiblue").getDoubleArray(new double[11]) + : m_limelightTable.getEntry("botpose_wpired").getDoubleArray(new double[11]); + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the robot in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose getRobotPoseInTargetSpace() { + var poseData = m_limelightTable.getEntry("botpose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the camera in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose getCameraPoseInTargetSpace() { + var poseData = m_limelightTable.getEntry("camerapose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the camera in the coordinate system of the robot + */ + public LimelightPose getCameraPoseInRobotSpace() { + var poseData = m_limelightTable.getEntry("camerapose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera + */ + public LimelightPose getTargetPoseInCameraSpace() { + var poseData = m_limelightTable.getEntry("targetpose_cameraspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot + */ + public LimelightPose getTargetPoseInRobotSpace() { + var poseData = m_limelightTable.getEntry("targetpose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * Calculates a trust value based on the number of tags in view. + * @return + */ + public Matrix calculateTrust(double tagCount) { + // Trust level is a function of the number of tags in view + // var trustLevel = 0.490956d + Math.pow(9998.51d, -(6.95795d * tagCount)); + + var trustLevel = tagCount >= 2.0 ? 2 : 20; + + // X Y Z trust levels (never trust Z) + return VecBuilder.fill(trustLevel, trustLevel, 999999); + } + + //#endregion + + //#region Camera Controls + + /** + * Sets limelight’s LED state. + * 0 = use the LED Mode set in the current pipeline. + * 1 = force off. + * 2 = force blink. + * 3 = force on. + * @param mode + */ + public void setLedMode(int mode) { + m_limelightTable.getEntry("ledMode").setNumber(mode); + } + + /** + * Forces the LED to blink a specified number of times, then returns to pipeline control. + */ + public void blinkLed(int blinkCount) { + m_executorService.submit(() -> { + // Blink the LED X times with 100ms on, 200ms off for each blink + for (int i = 0; i < blinkCount; i++) { + m_limelightTable.getEntry("ledMode").setNumber(3); + + try { + Thread.sleep(100); + } catch (Exception e) { + Thread.currentThread().interrupt(); + } + + m_limelightTable.getEntry("ledMode").setNumber(1); + try { + Thread.sleep(200); + } catch (Exception e) { + Thread.currentThread().interrupt(); + } + } + + // Then return to pipeline control + setLedMode(0); + }); + } + + /** + * Sets limelight’s operation mode. + * 0 = Vision processor. + * 1 = Driver Camera (Increases exposure, disables vision processing). + * @param mode + */ + public void setCameraMode(int mode) { + m_limelightTable.getEntry("camMode").setNumber(mode); + } + + /** + * Sets limelight’s pipeline. + * @param pipeline + */ + public void setPipeline(int pipeline) { + m_limelightTable.getEntry("pipeline").setNumber(pipeline); + } + + /** + * Side-by-side streams (Note: USB output stream is not affected by this mode) + * @param mode + */ + public void setPiPStreamingMode(int mode) { + m_limelightTable.getEntry("stream").setNumber(mode); + } + + /** + * Allows users to take snapshots during a match + */ + public void takeSnapshot() { + m_limelightTable.getEntry("snapshot").setNumber(0); // Reset + m_limelightTable.getEntry("snapshot").setNumber(1); // Take snapshot + m_limelightTable.getEntry("snapshot").setNumber(0); // Reset + } + + /** + * Set the camera's pose in the coordinate system of the robot. + * @param pose + */ + public void setCameraPose(Pose3d pose) { + var poseData = new double[] { + pose.getTranslation().getX(), + pose.getTranslation().getY(), + pose.getTranslation().getZ(), + Units.radiansToDegrees(pose.getRotation().getX()), + Units.radiansToDegrees(pose.getRotation().getY()), + Units.radiansToDegrees(pose.getRotation().getZ()), + }; + + m_limelightTable.getEntry("camerapose_robotspace_set").setDoubleArray(poseData); + } + + //#endregion + + public void close() { + m_executorService.shutdown(); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..873c876 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,122 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import prime.vision.LimelightInputs; + +@Logged(strategy = Strategy.OPT_IN) +public class VisionSubsystem extends SubsystemBase { + private LimeLightNT[] m_limelights; + + @Logged(name = "LimelightInputs", importance = Logged.Importance.CRITICAL) + private LimelightInputs[] m_limelightInputs; + + public VisionSubsystem(String[] tableNames) { + setName("VisionSubsystem"); + var defaultInstance = NetworkTableInstance.getDefault(); + m_limelights = new LimeLightNT[tableNames.length]; + for (int i = 0; i < tableNames.length; i++) { + m_limelights[i] = new LimeLightNT(defaultInstance, tableNames[i]); + } + + m_limelightInputs = new LimelightInputs[tableNames.length]; + } + + /** + * Gets the inputs for the specified limelight. + * @param llIndex The index of the limelight to get inputs from. + * @return + */ + public LimelightInputs getLimelightInputs(int llIndex) { + return m_limelightInputs[llIndex]; + } + + /** + * Sets limelight’s LED state. + * 0 = use the LED Mode set in the current pipeline. + * 1 = force off. + * 2 = force blink. + * 3 = force on. + * @param llIndex The index of the desired limelight + * @param mode The LED mode to set + */ + public void setLedMode(int llIndex, int mode) { + m_limelights[llIndex].setLedMode(mode); + } + + /** + * Forces the LED to blink a specified number of times, then returns to pipeline control. + * @param llIndex The index of the desired limelight + * @param blinkCount The number of times to blink the LED + */ + public void blinkLed(int llIndex, int blinkCount) { + m_limelights[llIndex].blinkLed(blinkCount); + } + + /** + * Sets limelight’s operation mode. + * 0 = Vision processor. + * 1 = Driver Camera (Increases exposure, disables vision processing). + * @param llIndex The index of the desired limelight + * @param mode The camera mode to set + */ + public void setCameraMode(int llIndex, int mode) { + m_limelights[llIndex].setCameraMode(mode); + } + + /** + * Sets limelight’s active vision pipeline. + * @param llIndex The index of the desired limelight + * @param pipeline The pipeline to set active + */ + public void setPipeline(int llIndex, int pipeline) { + m_limelights[llIndex].setPipeline(pipeline); + } + + /** + * Sets limelight’s streaming mode. + * 0 = Standard - Side-by-side streams if a webcam is attached to Limelight + * 1 = PiP Main - The secondary camera stream is placed in the lower-right corner of the primary camera stream + * 2 = PiP Secondary - The primary camera stream is placed in the lower-right corner of the secondary camera stream + * @param llIndex The index of the desired limelight + * @param mode The streaming mode to set + */ + public void setPiPStreamingMode(int llIndex, int mode) { + m_limelights[llIndex].setPiPStreamingMode(mode); + } + + /** + * Takes an instantaneous snapshot of the limelight's camera feed. + * @param llIndex The index of the desired limelight + */ + public void takeSnapshot(int llIndex) { + m_limelights[llIndex].takeSnapshot(); + } + + /** + * Set the camera's pose in the coordinate system of the robot. + * @param llIndex The index of the desired limelight + * @param pose The Camera's pose to set in Robot space + */ + public void setCameraPose(int llIndex, Pose3d pose) { + m_limelights[llIndex].setCameraPose(pose); + } + + public void periodic() { + // Update all limelight inputs + for (int i = 0; i < m_limelights.length; i++) { + m_limelightInputs[i] = m_limelights[i].getInputs(); + } + } + + public boolean isSpeakerCenterTarget(int apriltagId) { + return apriltagId == 4 || apriltagId == 7; + } + + public boolean isValidApriltag(int apriltagId) { + return apriltagId >= 1 && apriltagId <= 16; + } +} diff --git a/src/main/java/prime/physics/LimelightPose.java b/src/main/java/prime/physics/LimelightPose.java index 6dd5038..5e7d4ba 100644 --- a/src/main/java/prime/physics/LimelightPose.java +++ b/src/main/java/prime/physics/LimelightPose.java @@ -1,5 +1,10 @@ package prime.physics; +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -7,20 +12,23 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.struct.StructSerializable; import edu.wpi.first.wpilibj.Timer; -public class LimelightPose { +@Logged(strategy = Strategy.OPT_OUT) +public class LimelightPose implements StructSerializable { - public Pose3d Pose; - public double Timestamp; - public double TagCount; - public double TagSpan; - public double AvgTagDistanceMeters; - public double AvgTagArea; - public Matrix StdDeviations; + public Pose3d Pose = new Pose3d(); + public double Timestamp = 0.0; + public double TagCount = 0.0; + public double TagSpan = 0.0; + public double AvgTagDistanceMeters = 0.0; + public double AvgTagArea = 0.0; + @NotLogged + public Matrix StdDeviations = new Matrix<>(new SimpleMatrix(3, 1)); public LimelightPose(double[] data, Matrix stdDeviations) { - if (data.length < 6) { + if (data.length < 11 || data.length > 11) { System.err.println("Bad LL 3D Pose Data!"); return; } @@ -43,4 +51,24 @@ public LimelightPose(double[] data, Matrix stdDeviations) { AvgTagArea = data[10]; StdDeviations = stdDeviations; } + + public LimelightPose(Pose3d pose, double[] data, Matrix stdDeviations) { + if (data.length < 5 || data.length > 5) { + System.err.println("Bad LL 3D Pose Data!"); + return; + } + + Pose = pose; + + var latencyMs = data[0]; + Timestamp = Timer.getFPGATimestamp() - (latencyMs / 1000.0); + TagCount = data[1]; + TagSpan = data[2]; + AvgTagDistanceMeters = data[3]; + AvgTagArea = data[4]; + StdDeviations = stdDeviations; + } + + /** Struct for serialization. */ + public static final LimelightPoseStruct struct = new LimelightPoseStruct(); } diff --git a/src/main/java/prime/physics/LimelightPoseStruct.java b/src/main/java/prime/physics/LimelightPoseStruct.java new file mode 100644 index 0000000..96094fa --- /dev/null +++ b/src/main/java/prime/physics/LimelightPoseStruct.java @@ -0,0 +1,62 @@ +package prime.physics; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.util.struct.Struct; + +public class LimelightPoseStruct implements Struct { + @Override + public Class getTypeClass() { + return LimelightPose.class; + } + + @Override + public String getTypeName() { + return "LimelightPose"; + } + + @Override + public int getSize() { + var size = Pose3d.struct.getSize() + (kSizeDouble * 5) + Matrix.getStruct(Nat.N3(), Nat.N1()).getSize(); + + return size; + } + + @Override + public String getSchema() { + return "limelight pose"; + } + + @Override + public LimelightPose unpack(ByteBuffer bb) { + var pose = Pose3d.struct.unpack(bb); + + var data = new double[5]; + for (int i = 0; i < 5; i++) { + data[i] = bb.getDouble(i * kSizeDouble); + } + + var stdDeviations = Matrix.getStruct(Nat.N3(), Nat.N1()).unpack(bb.position(kSizeDouble * 11)); + + return new LimelightPose(pose, data, stdDeviations); + } + + @Override + public void pack(ByteBuffer bb, LimelightPose value) { + Pose3d.struct.pack(bb, value.Pose); + bb.putDouble(value.Timestamp); + bb.putDouble(value.TagCount); + bb.putDouble(value.TagSpan); + bb.putDouble(value.AvgTagDistanceMeters); + bb.putDouble(value.AvgTagArea); + Matrix.getStruct(Nat.N3(), Nat.N1()).pack(bb, value.StdDeviations); + } + + @Override + public boolean isImmutable() { + return true; + } +} diff --git a/src/main/java/prime/vision/LimelightInputs.java b/src/main/java/prime/vision/LimelightInputs.java new file mode 100644 index 0000000..fd8d054 --- /dev/null +++ b/src/main/java/prime/vision/LimelightInputs.java @@ -0,0 +1,96 @@ +package prime.vision; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.struct.StructSerializable; +import prime.physics.LimelightPose; + +@Logged +public class LimelightInputs implements StructSerializable { + + /** + * Horizontal Offset From Crosshair To Target + * (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) + */ + public Rotation2d TargetHorizontalOffset = new Rotation2d(); + + /** + * Vertical Offset From Crosshair To Target + * (LL1: -20.5 degrees to 20.5 degrees / LL2: -24.85 to 24.85 degrees) + */ + public Rotation2d TargetVerticalOffset = new Rotation2d(); + + /** + * Target Area (0% of image to 100% of image) + */ + public double TargetArea = 0.0; + + /** + * The pipeline's latency contribution (ms). Add to "cl" to get total latency. + */ + public long PipelineLatencyMs = 0; + + /** + * Time between the end of the exposure of the middle row of the sensor to + * the beginning of the tracking pipeline. + */ + public long CapturePipelineLatencyMs = 0; + + /** + * The total latency of the capture and pipeline processing in milliseconds. + */ + public long TotalLatencyMs = 0; + + /** + * ID of the primary in-view AprilTag + */ + public int ApriltagId = -1; + + /** + * Returns the number of AprilTags in the image. + */ + public double TagCount = 0.0; + + /** + * Robot transform in field-space. + */ + public LimelightPose FieldSpaceRobotPose = null; + + /** + * Robot transform in field-space (alliance driverstation WPILIB origin). + */ + public LimelightPose RedAllianceOriginFieldSpaceRobotPose = null; + + /** + * Robot transform in field-space (alliance driverstation WPILIB origin). + */ + public LimelightPose BlueAllianceOriginFieldSpaceRobotPose = null; + + /** + * 3D transform of the robot in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose TargetSpaceRobotPose = null; + + /** + * 3D transform of the camera in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose TargetSpaceCameraPose = null; + + /** + * 3D transform of the camera in the coordinate system of the robot + */ + public LimelightPose RobotSpaceCameraPose = null; + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera + */ + public LimelightPose CameraSpaceTargetPose = null; + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot + */ + public LimelightPose RobotSpaceTargetPose = null; + + /** Struct for serialization. */ + public static final LimelightInputsStruct struct = new LimelightInputsStruct(); +} diff --git a/src/main/java/prime/vision/LimelightInputsStruct.java b/src/main/java/prime/vision/LimelightInputsStruct.java new file mode 100644 index 0000000..a7a285a --- /dev/null +++ b/src/main/java/prime/vision/LimelightInputsStruct.java @@ -0,0 +1,86 @@ +package prime.vision; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.struct.Struct; +import prime.physics.LimelightPose; + +public class LimelightInputsStruct implements Struct { + @Override + public Class getTypeClass() { + return LimelightInputs.class; + } + + @Override + public String getTypeName() { + return "LimelightInputs"; + } + + @Override + public int getSize() { + // Calculate the size based on the fields in LimelightInputs + int size = Rotation2d.struct.getSize() * 2; // For TargetHorizontalOffset and TargetVerticalOffset + size += kSizeDouble; // For TargetArea + size += kSizeInt64 * 3; // For PipelineLatencyMs, CapturePipelineLatencyMs, TotalLatencyMs + size += kSizeInt32; // For ApriltagId + size += kSizeDouble; // For TagCount + size += LimelightPose.struct.getSize() * 8; // For all poses + + return size; + } + + @Override + public String getSchema() { + return "limelight inputs"; + } + + @Override + public LimelightInputs unpack(ByteBuffer bb) { + LimelightInputs inputs = new LimelightInputs(); + + inputs.TargetHorizontalOffset = Rotation2d.struct.unpack(bb); + inputs.TargetVerticalOffset = Rotation2d.struct.unpack(bb); + inputs.TargetArea = bb.getDouble(); + inputs.PipelineLatencyMs = bb.getLong(); + inputs.CapturePipelineLatencyMs = bb.getLong(); + inputs.TotalLatencyMs = bb.getLong(); + inputs.ApriltagId = bb.getInt(); + inputs.TagCount = bb.getDouble(); + inputs.FieldSpaceRobotPose = LimelightPose.struct.unpack(bb); + inputs.RedAllianceOriginFieldSpaceRobotPose = LimelightPose.struct.unpack(bb); + inputs.BlueAllianceOriginFieldSpaceRobotPose = LimelightPose.struct.unpack(bb); + inputs.TargetSpaceRobotPose = LimelightPose.struct.unpack(bb); + inputs.TargetSpaceCameraPose = LimelightPose.struct.unpack(bb); + inputs.RobotSpaceCameraPose = LimelightPose.struct.unpack(bb); + inputs.CameraSpaceTargetPose = LimelightPose.struct.unpack(bb); + inputs.RobotSpaceTargetPose = LimelightPose.struct.unpack(bb); + + return inputs; + } + + @Override + public void pack(ByteBuffer bb, LimelightInputs value) { + Rotation2d.struct.pack(bb, value.TargetHorizontalOffset); + Rotation2d.struct.pack(bb, value.TargetVerticalOffset); + bb.putDouble(value.TargetArea); + bb.putLong(value.PipelineLatencyMs); + bb.putLong(value.CapturePipelineLatencyMs); + bb.putLong(value.TotalLatencyMs); + bb.putInt(value.ApriltagId); + bb.putDouble(value.TagCount); + LimelightPose.struct.pack(bb, value.FieldSpaceRobotPose); + LimelightPose.struct.pack(bb, value.RedAllianceOriginFieldSpaceRobotPose); + LimelightPose.struct.pack(bb, value.BlueAllianceOriginFieldSpaceRobotPose); + LimelightPose.struct.pack(bb, value.TargetSpaceRobotPose); + LimelightPose.struct.pack(bb, value.TargetSpaceCameraPose); + LimelightPose.struct.pack(bb, value.RobotSpaceCameraPose); + LimelightPose.struct.pack(bb, value.CameraSpaceTargetPose); + LimelightPose.struct.pack(bb, value.RobotSpaceTargetPose); + } + + @Override + public boolean isImmutable() { + return true; + } +} From acd5ca65d86bb20128aa9bdcd15ebb4cae0c5608 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Mon, 28 Oct 2024 23:36:20 -0500 Subject: [PATCH 05/24] renamed RobotContainer to simply "Container". Made members of DriverDashboard static. Refactored pose estimation for simplification. Moved Vision dashboard updates to vision subsys --- .../{RobotContainer.java => Container.java} | 24 ++- .../{subsystems => }/DriverDashboard.java | 32 ++-- src/main/java/frc/robot/Robot.java | 6 +- .../java/frc/robot/subsystems/Climbers.java | 13 +- .../drivetrain/DrivetrainSubsystem.java | 165 +++++++----------- .../swervemodule/SwerveModuleIOReal.java | 2 +- .../subsystems/vision/VisionSubsystem.java | 25 ++- .../java/prime/physics/LimelightPose.java | 3 +- .../{utilities => util}/CTREConverter.java | 2 +- .../{utilities => util}/REVConverter.java | 2 +- 10 files changed, 121 insertions(+), 153 deletions(-) rename src/main/java/frc/robot/{RobotContainer.java => Container.java} (91%) rename src/main/java/frc/robot/{subsystems => }/DriverDashboard.java (78%) rename src/main/java/prime/{utilities => util}/CTREConverter.java (99%) rename src/main/java/prime/{utilities => util}/REVConverter.java (99%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/Container.java similarity index 91% rename from src/main/java/frc/robot/RobotContainer.java rename to src/main/java/frc/robot/Container.java index def9679..fb177ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/Container.java @@ -16,55 +16,53 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + import frc.robot.subsystems.*; import frc.robot.subsystems.drivetrain.DriveMap; import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; -import java.util.Map; import prime.control.Controls; import prime.control.HolonomicControlStyle; import prime.control.PrimeXboxController; @Logged(strategy = Strategy.OPT_IN) -public class RobotContainer { +public class Container { private PrimeXboxController m_driverController; private PrimeXboxController m_operatorController; + @Logged(importance = Importance.CRITICAL) public VisionSubsystem Vision; - @Logged(name = "Drivetrain", importance = Importance.CRITICAL) + @Logged(importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; public Shooter Shooter; public Intake Intake; public Climbers Climbers; public PwmLEDs LEDs; public Compressor Compressor; - public DriverDashboard DriverDashboard; private CombinedCommands m_combinedCommands; - public RobotContainer(boolean isReal) { + public Container(boolean isReal) { try { + DriverDashboard.init(); m_driverController = new PrimeXboxController(Controls.DRIVER_PORT); m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); // Create new subsystems LEDs = new PwmLEDs(); - DriverDashboard = new DriverDashboard(); Vision = new VisionSubsystem(new String[] { DriveMap.LimelightFrontName, DriveMap.LimelightRearName }); - Drivetrain = new DrivetrainSubsystem(isReal, LEDs, DriverDashboard, Vision); + Drivetrain = new DrivetrainSubsystem(isReal, + LEDs::restorePersistentStripPattern, + LEDs::setStripTemporaryPattern, + Vision::getAllLimelightInputs); Shooter = new Shooter(LEDs); Intake = new Intake(); - Climbers = new Climbers(DriverDashboard); + Climbers = new Climbers(); Compressor = new Compressor(30, PneumaticsModuleType.REVPH); Compressor.enableDigital(); diff --git a/src/main/java/frc/robot/subsystems/DriverDashboard.java b/src/main/java/frc/robot/DriverDashboard.java similarity index 78% rename from src/main/java/frc/robot/subsystems/DriverDashboard.java rename to src/main/java/frc/robot/DriverDashboard.java index 91e4a65..9c0dcad 100644 --- a/src/main/java/frc/robot/subsystems/DriverDashboard.java +++ b/src/main/java/frc/robot/DriverDashboard.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems; +package frc.robot; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; @@ -14,12 +14,12 @@ public class DriverDashboard { - public ShuffleboardTab DriverTab = Shuffleboard.getTab("Driver"); - public ShuffleboardTab AutoTab = Shuffleboard.getTab("Auto Commands"); + public static ShuffleboardTab DriverTab = Shuffleboard.getTab("Driver"); + public static ShuffleboardTab AutoTab = Shuffleboard.getTab("Auto Commands"); - public UsbCamera m_frontColorCam; - public SendableChooser AutoChooser; - public GenericEntry AllianceBox = DriverTab + public static UsbCamera m_frontColorCam; + public static SendableChooser AutoChooser; + public static GenericEntry AllianceBox = DriverTab .add("Alliance", false) .withPosition(15, 0) .withSize(2, 3) @@ -28,40 +28,40 @@ public class DriverDashboard { .getEntry(); // Drive - public Field2d FieldWidget = new Field2d(); - public GenericEntry HeadingGyro = DriverTab + public static Field2d FieldWidget = new Field2d(); + public static GenericEntry HeadingGyro = DriverTab .add("Current Heading", 0) .withWidget(BuiltInWidgets.kGyro) .withPosition(12, 0) .withSize(3, 3) .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) .getEntry(); - public GenericEntry RearApTagIdField = DriverTab + public static GenericEntry RearApTagIdField = DriverTab .add("Rear APTag", 0) .withWidget(BuiltInWidgets.kTextView) .withPosition(12, 3) .withSize(2, 1) .getEntry(); - public GenericEntry FrontApTagIdField = DriverTab + public static GenericEntry FrontApTagIdField = DriverTab .add("Front APTag", 0) .withWidget(BuiltInWidgets.kTextView) .withPosition(14, 3) .withSize(2, 1) .getEntry(); - public GenericEntry RearApTagOffsetDial = DriverTab + public static GenericEntry RearApTagOffsetDial = DriverTab .add("Rear APTag X Offset", 0) .withWidget(BuiltInWidgets.kDial) .withProperties(Map.of("Min", -29.8, "Max", 29.8)) .withPosition(12, 4) .withSize(2, 3) .getEntry(); - public GenericEntry FrontPoseEstimationSwitch = DriverTab + public static GenericEntry FrontPoseEstimationSwitch = DriverTab .add("F Pose Est.", true) .withWidget(BuiltInWidgets.kToggleSwitch) .withPosition(14, 4) .withSize(2, 1) .getEntry(); - public GenericEntry RearPoseEstimationSwitch = DriverTab + public static GenericEntry RearPoseEstimationSwitch = DriverTab .add("R Pose Est.", true) .withWidget(BuiltInWidgets.kToggleSwitch) .withPosition(14, 5) @@ -69,7 +69,7 @@ public class DriverDashboard { .getEntry(); // Climbers - public GenericEntry ClimberControlsActiveBox = DriverTab + public static GenericEntry ClimberControlsActiveBox = DriverTab .add("Climbers Enabled", false) .withWidget(BuiltInWidgets.kBooleanBox) .withPosition(5, 6) @@ -80,7 +80,7 @@ public class DriverDashboard { * Constructs a new DriverDashboard and adds complex widgets that must be created in the constructor * @param config */ - public DriverDashboard() { + public static void init() { // DriverTab // .addCamera( // "Rear Limelight", @@ -110,7 +110,7 @@ public DriverDashboard() { * Adds an auto chooser to the Shuffleboard and configures it * @param chooser */ - public void addAutoChooser(SendableChooser chooser) { + public static void addAutoChooser(SendableChooser chooser) { AutoChooser = chooser; DriverTab.add(AutoChooser).withWidget(BuiltInWidgets.kComboBoxChooser).withPosition(0, 6).withSize(5, 2); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7d61eda..4cf5a8c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,7 +26,7 @@ public class Robot extends TimedRobot { @Logged(name = "Robot/Container", importance = Importance.CRITICAL) - private RobotContainer m_robotContainer; + private Container m_robotContainer; private Command m_autonomousCommand; public Robot() { @@ -53,7 +53,7 @@ public Robot() { Epilogue.bind(this); // Initialize the robot container - m_robotContainer = new RobotContainer(isReal()); + m_robotContainer = new Container(isReal()); } @Override @@ -69,7 +69,7 @@ public void disabledInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); - m_robotContainer.DriverDashboard.AllianceBox.setBoolean(onRedAlliance()); + DriverDashboard.AllianceBox.setBoolean(onRedAlliance()); } /** diff --git a/src/main/java/frc/robot/subsystems/Climbers.java b/src/main/java/frc/robot/subsystems/Climbers.java index e1bebf4..85f83ee 100644 --- a/src/main/java/frc/robot/subsystems/Climbers.java +++ b/src/main/java/frc/robot/subsystems/Climbers.java @@ -15,6 +15,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.DriverDashboard; + import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -41,9 +43,6 @@ public enum Side { kRight, } - // Configuration - private DriverDashboard m_driverDashboard; - // Motors private VictorSPX m_leftVictorSPX; private VictorSPX m_rightVictorSPX; @@ -63,9 +62,7 @@ public enum Side { * Creates a new Climbers subsystem * @param config */ - public Climbers(DriverDashboard dashboard) { - m_driverDashboard = dashboard; - + public Climbers() { m_leftVictorSPX = new VictorSPX(VMap.VictorSPXLeftCanID); m_leftVictorSPX.configFactoryDefault(); m_leftVictorSPX.setInverted(VMap.LeftInverted); @@ -163,14 +160,12 @@ public void setClutch(Side side, boolean engaged) { public void periodic() { // d_leftLimitEntry.setBoolean(m_leftLimitSwitch.get()); // d_rightLimitEntry.setBoolean(m_rightLimitSwitch.get()); - m_driverDashboard.ClimberControlsActiveBox.setBoolean(m_climbControlsEnabled); + DriverDashboard.ClimberControlsActiveBox.setBoolean(m_climbControlsEnabled); // Level2 Logging SmartDashboard.putBoolean("Climbers/ControlsEnabled", m_climbControlsEnabled); - SmartDashboard.putNumber("Climbers/LeftMotorOutput", m_leftVictorSPX.getMotorOutputPercent()); SmartDashboard.putNumber("Climbers/RightMotorOutput", m_rightVictorSPX.getMotorOutputPercent()); - SmartDashboard.putBoolean("Climbers/LeftLimitSwitch", m_leftLimitSwitch.get()); SmartDashboard.putBoolean("Climbers/RightLimitSwitch", m_rightLimitSwitch.get()); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 8cc78ba..60d6e6d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.GenericEntry; @@ -16,28 +15,32 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.DriverDashboard; import frc.robot.Robot; -import frc.robot.subsystems.DriverDashboard; -import frc.robot.subsystems.PwmLEDs; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Map; +import java.util.function.Consumer; +import java.util.function.Supplier; + import prime.control.SwerveControlSuppliers; import prime.control.LEDs.Color; +import prime.control.LEDs.Patterns.LEDPattern; import prime.control.LEDs.Patterns.PulsePattern; import prime.control.LEDs.Patterns.SolidPattern; +import prime.vision.LimelightInputs; @Logged(strategy = Strategy.OPT_IN) public class DrivetrainSubsystem extends SubsystemBase { - private PwmLEDs m_leds; + private Runnable m_restoreLEDPersistentPatternFunc; + private Consumer m_setLEDTemporaryPatternFunc; // Shuffleboard Drivetrain tab configuration - private DriverDashboard m_driverDashboard; private ShuffleboardTab d_drivetrainTab = Shuffleboard.getTab("Drivetrain"); public GenericEntry d_snapToEnabledEntry = d_drivetrainTab .add("SnapTo Enabled", false) @@ -53,27 +56,35 @@ public class DrivetrainSubsystem extends SubsystemBase { .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) .getEntry(); - // IO and swerve modules + // IO private IDrivetrainIO m_driveio; + @Logged(name = "DriveInputs", importance = Logged.Importance.CRITICAL) private DrivetrainIOInputs m_inputs; @Logged(name = "DriveOutputs", importance = Logged.Importance.CRITICAL) private DrivetrainIOOutputs m_outputs; // Vision, Kinematics, odometry - public VisionSubsystem m_vision; - @Logged(name = "FrontVisionPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) + private Supplier m_limelightInputsSupplier; + @Logged(importance = Logged.Importance.CRITICAL) public boolean EstimatePoseUsingFrontCamera = true; - @Logged(name = "RearVisionPoseEstimationEnabled", importance = Logged.Importance.CRITICAL) + @Logged(importance = Logged.Importance.CRITICAL) public boolean EstimatePoseUsingRearCamera = true; + @Logged(importance = Logged.Importance.CRITICAL) + public boolean WithinPoseEstimationVelocity = true; /** * Creates a new Drivetrain. */ - public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverDashboard, VisionSubsystem vision) { + public DrivetrainSubsystem( + boolean isReal, + Runnable restoreLEDPersistentPatternFunc, + Consumer setLEDTemporaryPatternFunc, + Supplier limelightInputsSupplier) { setName("Drivetrain"); - m_leds = leds; - m_driverDashboard = driverDashboard; + + m_restoreLEDPersistentPatternFunc = restoreLEDPersistentPatternFunc; + m_setLEDTemporaryPatternFunc = setLEDTemporaryPatternFunc; // Create IO m_driveio = isReal @@ -82,7 +93,7 @@ public DrivetrainSubsystem(boolean isReal, PwmLEDs leds, DriverDashboard driverD m_inputs = m_driveio.getInputs(); m_outputs = new DrivetrainIOOutputs(); - m_vision = vision; + m_limelightInputsSupplier = limelightInputsSupplier; configurePathPlanner(); } @@ -99,17 +110,17 @@ private void configurePathPlanner() { } // Set up PP to feed current path poses to the field widget - PathPlannerLogging.setLogCurrentPoseCallback(pose -> m_driverDashboard.FieldWidget.setRobotPose(pose)); + PathPlannerLogging.setLogCurrentPoseCallback(pose -> DriverDashboard.FieldWidget.setRobotPose(pose)); PathPlannerLogging.setLogTargetPoseCallback(pose -> - m_driverDashboard.FieldWidget.getObject("target pose").setPose(pose) + DriverDashboard.FieldWidget.getObject("target pose").setPose(pose) ); - PathPlannerLogging.setLogActivePathCallback(poses -> m_driverDashboard.FieldWidget.getObject("path").setPoses(poses)); + PathPlannerLogging.setLogActivePathCallback(poses -> DriverDashboard.FieldWidget.getObject("path").setPoses(poses)); // Configure PathPlanner holonomic control AutoBuilder.configure( - this::getPose, - this::setEstimatorPose, - this::getRobotRelativeChassisSpeeds, + () -> m_inputs.EstimatedRobotPose, + m_driveio::setEstimatorPose, + () -> m_inputs.RobotRelativeChassisSpeeds, (speeds, feedForwards) -> drivePathPlanner(speeds), new PPHolonomicDriveController( DriveMap.PathingTranslationPid.toPIDConstants(), @@ -149,34 +160,12 @@ private void drivePathPlanner(ChassisSpeeds pathSpeeds) { m_outputs.DesiredChassisSpeeds = pathSpeeds; } - /** - * Gets the current pose of the drivetrain from odometry - */ - private Pose2d getPose() { - return m_inputs.EstimatedRobotPose; - } - - /** - * Resets the position of odometry to the input pose - * @param pose The pose to reset the estimator to - */ - private void setEstimatorPose(Pose2d pose) { - m_driveio.setEstimatorPose(pose); - } - - /** - * Gets the direction the robot is facing in degrees, CCW+ - */ - private double getHeadingDegrees() { - return m_inputs.GyroAngle.getDegrees(); - } - /** * Enabled/disables snap-to control */ private void setSnapToEnabled(boolean enabled) { m_outputs.SnapEnabled = enabled; - if (!enabled) m_leds.restorePersistentStripPattern(); + if (!enabled) m_restoreLEDPersistentPatternFunc.run(); } /** @@ -190,59 +179,18 @@ private void setSnapToSetpoint(double angle) { m_outputs.SnapSetpoint = Rotation2d.fromRadians(setpoint); } - /** - * Gets the current chassis speeds of the robot - */ - private ChassisSpeeds getRobotRelativeChassisSpeeds() { - return m_inputs.RobotRelativeChassisSpeeds; - } - /** * Evaluates the pose estimation using the limelight cameras */ - private void evaluatePoseEstimation() { - var currentSpeeds = getRobotRelativeChassisSpeeds(); - - var withinTrustedVelocity = - currentSpeeds.omegaRadiansPerSecond < 0.2 && // 1 rad/s is about 60 degrees/s - currentSpeeds.vxMetersPerSecond < 2 && - currentSpeeds.vyMetersPerSecond < 2; - SmartDashboard.putBoolean("Drive/PoseEstimation/WithinTrustedVelocity", withinTrustedVelocity); - - EstimatePoseUsingFrontCamera = m_driverDashboard.FrontPoseEstimationSwitch.getBoolean(false); - SmartDashboard.putBoolean("Drive/PoseEstimation/FrontEstimationEnabled", EstimatePoseUsingFrontCamera); - if (EstimatePoseUsingFrontCamera) { - // Front Limelight - // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator - var frontInputs = m_vision.getLimelightInputs(0); - var frontIsValidTarget = m_vision.isValidApriltag(frontInputs.ApriltagId); - m_driverDashboard.FrontApTagIdField.setDouble(frontInputs.ApriltagId); - SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", frontIsValidTarget); - - if (frontIsValidTarget && withinTrustedVelocity) { - var llPose = frontInputs.BlueAllianceOriginFieldSpaceRobotPose; - - m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); - } - } - - EstimatePoseUsingRearCamera = m_driverDashboard.RearPoseEstimationSwitch.getBoolean(false); - SmartDashboard.putBoolean("Drive/PoseEstimation/RearEstimationEnabled", EstimatePoseUsingRearCamera); - if (EstimatePoseUsingRearCamera) { - // Rear Limelight - // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator - var rearInputs = m_vision.getLimelightInputs(1); - var isValidTarget = m_vision.isValidApriltag(rearInputs.ApriltagId); - SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isValidTarget); - - m_driverDashboard.RearApTagIdField.setDouble(rearInputs.ApriltagId); - m_driverDashboard.RearApTagOffsetDial.setDouble(rearInputs.TargetHorizontalOffset.getDegrees()); + private void evaluatePoseEstimation(boolean withinTrustedVelocity, int limelightIndex) { - if (isValidTarget && withinTrustedVelocity) { - var llPose = rearInputs.BlueAllianceOriginFieldSpaceRobotPose; + // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator + var limelightInputs = m_limelightInputsSupplier.get()[limelightIndex]; + var isValidTarget = VisionSubsystem.isAprilTagIdValid(limelightInputs.ApriltagId); + if (isValidTarget && withinTrustedVelocity) { + var llPose = limelightInputs.BlueAllianceOriginFieldSpaceRobotPose; - m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); - } + m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); } } @@ -253,23 +201,34 @@ private void evaluatePoseEstimation() { */ @Override public void periodic() { + // Get inputs + m_inputs = m_driveio.getInputs(); + // Pose estimation - evaluatePoseEstimation(); + WithinPoseEstimationVelocity = + m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond < 0.2 && // 1 rad/s is about 60 degrees/s + m_inputs.RobotRelativeChassisSpeeds.vxMetersPerSecond < 2 && + m_inputs.RobotRelativeChassisSpeeds.vyMetersPerSecond < 2; - // IO - m_inputs = m_driveio.getInputs(); - m_driveio.setOutputs(m_outputs); + EstimatePoseUsingFrontCamera = DriverDashboard.FrontPoseEstimationSwitch.getBoolean(false); + if (EstimatePoseUsingFrontCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 0); + + EstimatePoseUsingRearCamera = DriverDashboard.RearPoseEstimationSwitch.getBoolean(false); + if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); // Update LEDs if (m_inputs.SnapOnTarget) { - m_leds.setStripTemporaryPattern(new SolidPattern(Color.GREEN)); + m_setLEDTemporaryPatternFunc.accept(new SolidPattern(Color.GREEN)); } else { - m_leds.setStripTemporaryPattern(new PulsePattern(Color.RED, 0.5)); + m_setLEDTemporaryPatternFunc.accept(new PulsePattern(Color.RED, 0.5)); } + // Send outputs to the drive IO + m_driveio.setOutputs(m_outputs); + // Update shuffleboard - m_driverDashboard.HeadingGyro.setDouble(m_inputs.GyroAngle.getDegrees()); - m_driverDashboard.FieldWidget.setRobotPose(m_inputs.EstimatedRobotPose); + DriverDashboard.HeadingGyro.setDouble(m_inputs.GyroAngle.getDegrees()); + DriverDashboard.FieldWidget.setRobotPose(m_inputs.EstimatedRobotPose); d_snapToEnabledEntry.setBoolean(m_outputs.SnapEnabled); d_snapAngle.setDouble(m_outputs.SnapSetpoint.getRadians()); } @@ -285,7 +244,7 @@ public Command defaultDriveCommand(SwerveControlSuppliers controlSuppliers) { // If the driver is trying to rotate the robot, disable snap-to control if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { setSnapToEnabled(false); - m_leds.restorePersistentStripPattern(); + m_restoreLEDPersistentPatternFunc.run(); } // Convert inputs to MPS @@ -337,13 +296,13 @@ public Command disableSnapToCommand() { */ public Command enableLockOn() { return Commands.run(() -> { - var rearLimelightInputs = m_vision.getLimelightInputs(1); + var rearLimelightInputs = m_limelightInputsSupplier.get()[1]; // If targeted AprilTag is in validTargets, snap to its offset - if (m_vision.isSpeakerCenterTarget(rearLimelightInputs.ApriltagId)) { + if (VisionSubsystem.isAprilTagIdASpeakerCenterTarget(rearLimelightInputs.ApriltagId)) { // Calculate the target heading var horizontalOffsetDeg = rearLimelightInputs.TargetHorizontalOffset.getDegrees(); - var robotHeadingDeg = getHeadingDegrees(); + var robotHeadingDeg = m_inputs.GyroAngle.getDegrees(); var targetHeadingDeg = robotHeadingDeg - horizontalOffsetDeg; // Set the drivetrain to snap to the target heading diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java index af26497..f49d0f5 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java @@ -21,7 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.subsystems.drivetrain.DriveMap; import prime.control.PrimePIDConstants; -import prime.utilities.CTREConverter; +import prime.util.CTREConverter; @Logged(strategy = Strategy.OPT_IN) public class SwerveModuleIOReal implements ISwerveModuleIO { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 873c876..7b4c31e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -4,14 +4,16 @@ import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.DriverDashboard; import prime.vision.LimelightInputs; @Logged(strategy = Strategy.OPT_IN) public class VisionSubsystem extends SubsystemBase { private LimeLightNT[] m_limelights; - @Logged(name = "LimelightInputs", importance = Logged.Importance.CRITICAL) + @Logged(importance = Logged.Importance.CRITICAL) private LimelightInputs[] m_limelightInputs; public VisionSubsystem(String[] tableNames) { @@ -28,12 +30,18 @@ public VisionSubsystem(String[] tableNames) { /** * Gets the inputs for the specified limelight. * @param llIndex The index of the limelight to get inputs from. - * @return */ public LimelightInputs getLimelightInputs(int llIndex) { return m_limelightInputs[llIndex]; } + /** + * Gets all limelight inputs + */ + public LimelightInputs[] getAllLimelightInputs() { + return m_limelightInputs; + } + /** * Sets limelight’s LED state. * 0 = use the LED Mode set in the current pipeline. @@ -110,13 +118,22 @@ public void periodic() { for (int i = 0; i < m_limelights.length; i++) { m_limelightInputs[i] = m_limelights[i].getInputs(); } + + // Update Dashboard & logging + var frontInputs = getLimelightInputs(0); + var rearInputs = getLimelightInputs(1); + SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", isAprilTagIdValid(frontInputs.ApriltagId)); + SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isAprilTagIdValid(rearInputs.ApriltagId)); + DriverDashboard.FrontApTagIdField.setDouble(frontInputs.ApriltagId); + DriverDashboard.RearApTagIdField.setDouble(rearInputs.ApriltagId); + DriverDashboard.RearApTagOffsetDial.setDouble(rearInputs.TargetHorizontalOffset.getDegrees()); } - public boolean isSpeakerCenterTarget(int apriltagId) { + public static boolean isAprilTagIdASpeakerCenterTarget(int apriltagId) { return apriltagId == 4 || apriltagId == 7; } - public boolean isValidApriltag(int apriltagId) { + public static boolean isAprilTagIdValid(int apriltagId) { return apriltagId >= 1 && apriltagId <= 16; } } diff --git a/src/main/java/prime/physics/LimelightPose.java b/src/main/java/prime/physics/LimelightPose.java index 5e7d4ba..327b876 100644 --- a/src/main/java/prime/physics/LimelightPose.java +++ b/src/main/java/prime/physics/LimelightPose.java @@ -4,7 +4,6 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; -import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -15,7 +14,7 @@ import edu.wpi.first.util.struct.StructSerializable; import edu.wpi.first.wpilibj.Timer; -@Logged(strategy = Strategy.OPT_OUT) +@Logged public class LimelightPose implements StructSerializable { public Pose3d Pose = new Pose3d(); diff --git a/src/main/java/prime/utilities/CTREConverter.java b/src/main/java/prime/util/CTREConverter.java similarity index 99% rename from src/main/java/prime/utilities/CTREConverter.java rename to src/main/java/prime/util/CTREConverter.java index cb3ca28..14e4003 100644 --- a/src/main/java/prime/utilities/CTREConverter.java +++ b/src/main/java/prime/util/CTREConverter.java @@ -1,4 +1,4 @@ -package prime.utilities; +package prime.util; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/prime/utilities/REVConverter.java b/src/main/java/prime/util/REVConverter.java similarity index 99% rename from src/main/java/prime/utilities/REVConverter.java rename to src/main/java/prime/util/REVConverter.java index 704d6db..604451e 100644 --- a/src/main/java/prime/utilities/REVConverter.java +++ b/src/main/java/prime/util/REVConverter.java @@ -1,4 +1,4 @@ -package prime.utilities; +package prime.util; import edu.wpi.first.math.geometry.Rotation2d; From 63a7a53dd20135359abd6fc2f3071b50af45f5f9 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Wed, 30 Oct 2024 13:27:14 -0500 Subject: [PATCH 06/24] rewrote PwmLEDs to new built-in pattern & ran pathplanner GUI to add settings --- .prettierrc.yaml | 1 + .vscode/settings.json | 10 +- simgui-ds.json | 92 +++++++++ .../pathplanner/autos/SpAmp -2N- S-A1.auto | 10 +- .../pathplanner/autos/SpAmp -3N- S-A1-M1.auto | 10 +- .../pathplanner/autos/SpCenter -2N- S-A2.auto | 10 +- .../autos/SpCenter -3N- S-A2-A1.auto | 10 +- .../autos/SpCenter -3N- S-A2-M2.auto | 10 +- .../autos/SpCenter -3N- S-A3-A2.auto | 10 +- .../autos/SpCenter -4N- S-A3-A2-A1.auto | 10 +- .../pathplanner/autos/SpCenter-A2-M3.auto | 10 +- .../autos/SpSource -1N- S-M5.Grab.auto | 10 +- .../pathplanner/autos/SpSource -2N- S-A3.auto | 10 +- .../pathplanner/autos/SpSource -2N- S-M4.auto | 10 +- .../autos/SpSource -3N- S-A3-M4.auto | 10 +- .../pathplanner/autos/Straight Park.auto | 10 +- .../deploy/pathplanner/paths/A1-SpAmp.path | 23 ++- .../deploy/pathplanner/paths/A1-SpCenter.path | 17 +- .../deploy/pathplanner/paths/A2-SpCenter.path | 21 +- .../deploy/pathplanner/paths/A3-SpCenter.path | 17 +- .../deploy/pathplanner/paths/A3-SpSource.path | 17 +- .../deploy/pathplanner/paths/Just Park.path | 26 +-- .../deploy/pathplanner/paths/M1-SpAmp.path | 28 +-- .../deploy/pathplanner/paths/M2-SpCenter.path | 30 +-- .../deploy/pathplanner/paths/M3-SpCenter.path | 30 +-- .../deploy/pathplanner/paths/M4-SpSource.path | 32 +-- .../deploy/pathplanner/paths/New Path.path | 24 +-- .../deploy/pathplanner/paths/SpAmp-A1.path | 20 +- .../deploy/pathplanner/paths/SpAmp-M1.path | 26 +-- .../deploy/pathplanner/paths/SpCenter-A1.path | 18 +- .../deploy/pathplanner/paths/SpCenter-A2.path | 18 +- .../deploy/pathplanner/paths/SpCenter-A3.path | 18 +- .../deploy/pathplanner/paths/SpCenter-M2.path | 32 +-- .../deploy/pathplanner/paths/SpCenter-M3.path | 34 ++-- .../pathplanner/paths/SpCenter-ParkAtA2.path | 16 +- .../deploy/pathplanner/paths/SpSource-A3.path | 18 +- .../deploy/pathplanner/paths/SpSource-M4.path | 28 +-- .../deploy/pathplanner/paths/SpSource-M5.path | 34 ++-- .../deploy/pathplanner/paths/line 1m.path | 19 +- src/main/deploy/pathplanner/settings.json | 21 ++ src/main/java/frc/robot/Container.java | 15 +- src/main/java/frc/robot/Robot.java | 24 ++- .../java/frc/robot/subsystems/PwmLEDs.java | 183 +++++++++++------- .../java/frc/robot/subsystems/Shooter.java | 39 ++-- .../drivetrain/DrivetrainIOInputs.java | 2 +- .../drivetrain/DrivetrainIOReal.java | 2 +- .../drivetrain/DrivetrainIOSim.java | 18 +- .../drivetrain/DrivetrainSubsystem.java | 33 ++-- .../swervemodule/SwerveModuleIOSim.java | 2 +- src/main/java/prime/control/LEDs/Color.java | 64 ------ .../java/prime/control/LEDs/LEDEffect.java | 8 - .../control/LEDs/Patterns/BlinkPattern.java | 34 ---- .../control/LEDs/Patterns/ChasePattern.java | 97 ---------- .../control/LEDs/Patterns/LEDPattern.java | 90 --------- .../control/LEDs/Patterns/PulsePattern.java | 68 ------- .../control/LEDs/Patterns/SolidPattern.java | 32 --- 56 files changed, 657 insertions(+), 854 deletions(-) create mode 100644 .prettierrc.yaml create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/pathplanner/settings.json delete mode 100644 src/main/java/prime/control/LEDs/Color.java delete mode 100644 src/main/java/prime/control/LEDs/LEDEffect.java delete mode 100644 src/main/java/prime/control/LEDs/Patterns/BlinkPattern.java delete mode 100644 src/main/java/prime/control/LEDs/Patterns/ChasePattern.java delete mode 100644 src/main/java/prime/control/LEDs/Patterns/LEDPattern.java delete mode 100644 src/main/java/prime/control/LEDs/Patterns/PulsePattern.java delete mode 100644 src/main/java/prime/control/LEDs/Patterns/SolidPattern.java diff --git a/.prettierrc.yaml b/.prettierrc.yaml new file mode 100644 index 0000000..62ac334 --- /dev/null +++ b/.prettierrc.yaml @@ -0,0 +1 @@ +printWidth: 120 \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..a505b56 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,13 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "[java]": { + "editor.formatOnSave": true, + "editor.formatOnPaste": true, + "editor.defaultFormatter": "esbenp.prettier-vscode", + "editor.codeActionsOnSave": [ + "editor.action.formatDocument", + ] + }, } diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/SpAmp -2N- S-A1.auto b/src/main/deploy/pathplanner/autos/SpAmp -2N- S-A1.auto index 30eb4e2..7e8735b 100644 --- a/src/main/deploy/pathplanner/autos/SpAmp -2N- S-A1.auto +++ b/src/main/deploy/pathplanner/autos/SpAmp -2N- S-A1.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.82, - "y": 6.48 - }, - "rotation": 60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -50,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Amp-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpAmp -3N- S-A1-M1.auto b/src/main/deploy/pathplanner/autos/SpAmp -3N- S-A1-M1.auto index 2627568..1797330 100644 --- a/src/main/deploy/pathplanner/autos/SpAmp -3N- S-A1-M1.auto +++ b/src/main/deploy/pathplanner/autos/SpAmp -3N- S-A1-M1.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.82, - "y": 6.48 - }, - "rotation": 60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -80,6 +73,7 @@ ] } }, + "resetOdom": true, "folder": "Amp-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter -2N- S-A2.auto b/src/main/deploy/pathplanner/autos/SpCenter -2N- S-A2.auto index 87b22b2..f11e4be 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter -2N- S-A2.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter -2N- S-A2.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3, - "y": 5.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -56,6 +49,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-A1.auto b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-A1.auto index 3c58e97..baf2876 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-A1.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-A1.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3, - "y": 5.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -86,6 +79,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-M2.auto b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-M2.auto index 648479c..26b275c 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-M2.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A2-M2.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3, - "y": 5.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -86,6 +79,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A3-A2.auto b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A3-A2.auto index 24ba4d3..6c957a6 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A3-A2.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter -3N- S-A3-A2.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3, - "y": 5.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -86,6 +79,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter -4N- S-A3-A2-A1.auto b/src/main/deploy/pathplanner/autos/SpCenter -4N- S-A3-A2-A1.auto index fc258ec..6fe116b 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter -4N- S-A3-A2-A1.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter -4N- S-A3-A2-A1.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3, - "y": 5.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -116,6 +109,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpCenter-A2-M3.auto b/src/main/deploy/pathplanner/autos/SpCenter-A2-M3.auto index c34527e..c134a2f 100644 --- a/src/main/deploy/pathplanner/autos/SpCenter-A2-M3.auto +++ b/src/main/deploy/pathplanner/autos/SpCenter-A2-M3.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.2997503472484422, - "y": 5.5 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -80,6 +73,7 @@ ] } }, + "resetOdom": true, "folder": "Center", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpSource -1N- S-M5.Grab.auto b/src/main/deploy/pathplanner/autos/SpSource -1N- S-M5.Grab.auto index c118bcf..c793823 100644 --- a/src/main/deploy/pathplanner/autos/SpSource -1N- S-M5.Grab.auto +++ b/src/main/deploy/pathplanner/autos/SpSource -1N- S-M5.Grab.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.6 - }, - "rotation": -60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -44,6 +37,7 @@ ] } }, + "resetOdom": true, "folder": "Source-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpSource -2N- S-A3.auto b/src/main/deploy/pathplanner/autos/SpSource -2N- S-A3.auto index 6978789..8d48e2f 100644 --- a/src/main/deploy/pathplanner/autos/SpSource -2N- S-A3.auto +++ b/src/main/deploy/pathplanner/autos/SpSource -2N- S-A3.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.6 - }, - "rotation": -60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -50,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Source-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpSource -2N- S-M4.auto b/src/main/deploy/pathplanner/autos/SpSource -2N- S-M4.auto index 134fe4a..f95d226 100644 --- a/src/main/deploy/pathplanner/autos/SpSource -2N- S-M4.auto +++ b/src/main/deploy/pathplanner/autos/SpSource -2N- S-M4.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.6 - }, - "rotation": -60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -50,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Source-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpSource -3N- S-A3-M4.auto b/src/main/deploy/pathplanner/autos/SpSource -3N- S-A3-M4.auto index 8a003c3..4c8c975 100644 --- a/src/main/deploy/pathplanner/autos/SpSource -3N- S-A3-M4.auto +++ b/src/main/deploy/pathplanner/autos/SpSource -3N- S-A3-M4.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.6 - }, - "rotation": -60.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -80,6 +73,7 @@ ] } }, + "resetOdom": true, "folder": "Source-Side", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Straight Park.auto b/src/main/deploy/pathplanner/autos/Straight Park.auto index 6ca87f3..18283f4 100644 --- a/src/main/deploy/pathplanner/autos/Straight Park.auto +++ b/src/main/deploy/pathplanner/autos/Straight Park.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4, - "y": 1.5 - }, - "rotation": 0.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -20,6 +13,7 @@ ] } }, + "resetOdom": true, "folder": "Park Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-SpAmp.path b/src/main/deploy/pathplanner/paths/A1-SpAmp.path index 54a7158..628b74c 100644 --- a/src/main/deploy/pathplanner/paths/A1-SpAmp.path +++ b/src/main/deploy/pathplanner/paths/A1-SpAmp.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.5493573329488999, + "x": 1.5493573329488997, "y": 7.06295322500555 }, "isLocked": false, @@ -20,8 +20,8 @@ "y": 6.48 }, "prevControl": { - "x": 0.9529834075884366, - "y": 6.609962769475088 + "x": 0.9987956499722008, + "y": 6.654734414329343 }, "nextControl": null, "isLocked": false, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -65,18 +67,19 @@ "maxVelocity": 2.5, "maxAcceleration": 2.5, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.91669743559589, - "rotateFast": true + "rotation": 59.916697435595886 }, "reversed": false, "folder": "Amp Side Auto Paths", - "previewStartingState": { - "rotation": -0.2312113625636056, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-SpCenter.path b/src/main/deploy/pathplanner/paths/A1-SpCenter.path index d9bd80d..7e107b7 100644 --- a/src/main/deploy/pathplanner/paths/A1-SpCenter.path +++ b/src/main/deploy/pathplanner/paths/A1-SpCenter.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -65,18 +67,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.10916378845756709, - "rotateFast": true + "rotation": 0.10916378845756709 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 35.917711121441634, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A2-SpCenter.path b/src/main/deploy/pathplanner/paths/A2-SpCenter.path index afe0ee5..39839de 100644 --- a/src/main/deploy/pathplanner/paths/A2-SpCenter.path +++ b/src/main/deploy/pathplanner/paths/A2-SpCenter.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 2.4286159882154656, + "x": 2.66, "y": 5.5 }, "isLocked": false, @@ -20,7 +20,7 @@ "y": 5.5 }, "prevControl": { - "x": 1.3354677213287052, + "x": 1.1, "y": 5.5 }, "nextControl": null, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Bring Intake in ", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -65,18 +67,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": true + "rotation": 0.0 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 1.9732012486582053, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A3-SpCenter.path b/src/main/deploy/pathplanner/paths/A3-SpCenter.path index 8b656e9..8621f37 100644 --- a/src/main/deploy/pathplanner/paths/A3-SpCenter.path +++ b/src/main/deploy/pathplanner/paths/A3-SpCenter.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -65,18 +67,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.08066959478685368, - "rotateFast": true + "rotation": 0.08066959478685368 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": -40.12822556926528, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A3-SpSource.path b/src/main/deploy/pathplanner/paths/A3-SpSource.path index 394b7a4..bf4e856 100644 --- a/src/main/deploy/pathplanner/paths/A3-SpSource.path +++ b/src/main/deploy/pathplanner/paths/A3-SpSource.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -65,18 +67,19 @@ "maxVelocity": 2.5, "maxAcceleration": 2.5, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -60.0703653420918, - "rotateFast": true + "rotation": -60.0703653420918 }, "reversed": false, "folder": "Source Side Auto Paths", - "previewStartingState": { - "rotation": 1.0686756693789388, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Just Park.path b/src/main/deploy/pathplanner/paths/Just Park.path index 5125232..94c3c35 100644 --- a/src/main/deploy/pathplanner/paths/Just Park.path +++ b/src/main/deploy/pathplanner/paths/Just Park.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -21,7 +21,7 @@ }, "prevControl": { "x": 2.3166418821455848, - "y": 1.5 + "y": 1.5000000000000002 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.7631436521121626, - "rotateFast": false + "rotation": 0.7631436521121626 }, "reversed": false, - "folder": "Park Auto Paths", - "previewStartingState": { - "rotation": 1.2167323560941719, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M1-SpAmp.path b/src/main/deploy/pathplanner/paths/M1-SpAmp.path index 130f304..7fba827 100644 --- a/src/main/deploy/pathplanner/paths/M1-SpAmp.path +++ b/src/main/deploy/pathplanner/paths/M1-SpAmp.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -20,7 +20,7 @@ "y": 6.48 }, "prevControl": { - "x": 1.886698428171631, + "x": 1.8866984281716315, "y": 6.933527380568275 }, "nextControl": null, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -57,6 +59,7 @@ { "name": "Run Shooter Up to Speed", "waypointRelativePos": 0.6, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -73,21 +76,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 60.67619189974157, - "rotateFast": true + "rotation": 60.676191899741575 }, "reversed": false, - "folder": "Amp Side Auto Paths", - "previewStartingState": { - "rotation": -1.190783827497893, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M2-SpCenter.path b/src/main/deploy/pathplanner/paths/M2-SpCenter.path index dd58e13..cff510a 100644 --- a/src/main/deploy/pathplanner/paths/M2-SpCenter.path +++ b/src/main/deploy/pathplanner/paths/M2-SpCenter.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -38,14 +38,18 @@ "maxVelocity": 5.0, "maxAcceleration": 2.25, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false } } ], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Bring Note In", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -69,6 +73,7 @@ { "name": "Run Shooter Up to Speed", "waypointRelativePos": 0.7, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -85,21 +90,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.03466879540972598, - "rotateFast": true + "rotation": 0.03466879540972598 }, "reversed": false, - "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": -14.210362244267992, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M3-SpCenter.path b/src/main/deploy/pathplanner/paths/M3-SpCenter.path index 03b6ccb..f7917fb 100644 --- a/src/main/deploy/pathplanner/paths/M3-SpCenter.path +++ b/src/main/deploy/pathplanner/paths/M3-SpCenter.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -38,14 +38,18 @@ "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false } } ], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Score", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -69,6 +73,7 @@ { "name": "Run Shooter Up To Speed", "waypointRelativePos": 0.65, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -85,21 +90,22 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": true + "rotation": 0.0 }, "reversed": false, - "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 0.05537491902364609, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M4-SpSource.path b/src/main/deploy/pathplanner/paths/M4-SpSource.path index ef3a48f..11b3979 100644 --- a/src/main/deploy/pathplanner/paths/M4-SpSource.path +++ b/src/main/deploy/pathplanner/paths/M4-SpSource.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 5.821165496204866, - "y": 0.13640465713363947 + "y": 0.1364046571336397 }, "isLocked": false, "linkedName": null @@ -38,14 +38,18 @@ "maxVelocity": 3.0, "maxAcceleration": 4.0, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false } } ], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Prepare to Shoot", "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -69,6 +73,7 @@ { "name": "Run Shooter Up to Speed", "waypointRelativePos": 0.65, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -85,21 +90,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.01296980266918, - "rotateFast": true + "rotation": -59.01296980266918 }, "reversed": false, - "folder": "Source Side Auto Paths", - "previewStartingState": { - "rotation": 19.776299750891862, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 8b8c8b5..0fbf25b 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": "Test Autos", - "previewStartingState": { - "rotation": 0.5256346064576232, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpAmp-A1.path b/src/main/deploy/pathplanner/paths/SpAmp-A1.path index 90ab5d2..1fa5041 100644 --- a/src/main/deploy/pathplanner/paths/SpAmp-A1.path +++ b/src/main/deploy/pathplanner/paths/SpAmp-A1.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -20,7 +20,7 @@ "y": 7.0 }, "prevControl": { - "x": 1.9838583821311786, + "x": 1.9838583821311784, "y": 6.998240302786913 }, "nextControl": null, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Bring out Intake", "waypointRelativePos": 0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.8, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -70,18 +73,19 @@ "maxVelocity": 2.5, "maxAcceleration": 2.5, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -0.32879961776107325, - "rotateFast": true + "rotation": -0.32879961776107325 }, "reversed": false, "folder": "Amp Side Auto Paths", - "previewStartingState": { - "rotation": 59.93162821859144, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpAmp-M1.path b/src/main/deploy/pathplanner/paths/SpAmp-M1.path index 333c50e..cef4a19 100644 --- a/src/main/deploy/pathplanner/paths/SpAmp-M1.path +++ b/src/main/deploy/pathplanner/paths/SpAmp-M1.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Set Intake Out", "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.9, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -67,21 +70,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.11435317127614646, - "rotateFast": true + "rotation": 0.11435317127614646 }, "reversed": false, - "folder": "Amp Side Auto Paths", - "previewStartingState": { - "rotation": 60.3619986852921, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-A1.path b/src/main/deploy/pathplanner/paths/SpCenter-A1.path index a5d223f..6813a21 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-A1.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-A1.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Intake Down", "waypointRelativePos": 0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.75, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -70,18 +73,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 33.79850324111527, - "rotateFast": true + "rotation": 33.79850324111527 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 0.05537491902364609, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-A2.path b/src/main/deploy/pathplanner/paths/SpCenter-A2.path index 789d4e2..1116711 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-A2.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-A2.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Bring out intake", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.9, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -70,18 +73,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 4.0, - "rotateFast": true + "rotation": 4.0 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": -0.228526796211064, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-A3.path b/src/main/deploy/pathplanner/paths/SpCenter-A3.path index b8815ec..0095d5d 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-A3.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-A3.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Intake Down", "waypointRelativePos": 0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.9, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -70,18 +73,19 @@ "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -36.94362459900666, - "rotateFast": true + "rotation": -36.94362459900666 }, "reversed": false, "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 0.10916378845756709, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-M2.path b/src/main/deploy/pathplanner/paths/SpCenter-M2.path index d9ad70b..57d7b50 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-M2.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-M2.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 4.807269334358488, + "x": 4.807269334358489, "y": 5.066097339401886 }, "isLocked": false, @@ -20,8 +20,8 @@ "y": 5.861141812373715 }, "prevControl": { - "x": 4.206832041592011, - "y": 8.32666174683701 + "x": 4.206832041592009, + "y": 8.326661746837008 }, "nextControl": null, "isLocked": false, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Set Intake Out", "waypointRelativePos": 0.45, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake note", "waypointRelativePos": 0.95, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -67,21 +70,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -21.370622269343173, - "rotateFast": true + "rotation": -21.370622269343173 }, "reversed": false, - "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": -0.6458111432507809, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-M3.path b/src/main/deploy/pathplanner/paths/SpCenter-M3.path index d4a4ae2..e17119c 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-M3.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-M3.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.9783901630866325, + "x": 1.9783901630866327, "y": 5.953588844114626 }, "isLocked": false, @@ -21,7 +21,7 @@ }, "prevControl": { "x": 6.125485404123961, - "y": 3.762594191855051 + "y": 3.7625941918550505 }, "nextControl": null, "isLocked": false, @@ -38,14 +38,18 @@ "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false } } ], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Intake Down", "waypointRelativePos": 0.75, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -63,6 +67,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.9, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -79,21 +84,22 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": true + "rotation": 0.0 }, "reversed": false, - "folder": "Center Auto Paths", - "previewStartingState": { - "rotation": 0.05537491902364609, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpCenter-ParkAtA2.path b/src/main/deploy/pathplanner/paths/SpCenter-ParkAtA2.path index 27950b2..7cb7207 100644 --- a/src/main/deploy/pathplanner/paths/SpCenter-ParkAtA2.path +++ b/src/main/deploy/pathplanner/paths/SpCenter-ParkAtA2.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.75, "maxAcceleration": 1.75, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": "Park Auto Paths", - "previewStartingState": { - "rotation": -0.19616892235128405, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpSource-A3.path b/src/main/deploy/pathplanner/paths/SpSource-A3.path index e3658d3..6caf5b9 100644 --- a/src/main/deploy/pathplanner/paths/SpSource-A3.path +++ b/src/main/deploy/pathplanner/paths/SpSource-A3.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Bring Intake Out", "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.85, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -70,18 +73,19 @@ "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 360.0 + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.5695686789390435, - "rotateFast": true + "rotation": 0.5695686789390435 }, "reversed": false, "folder": "Source Side Auto Paths", - "previewStartingState": { - "rotation": -60.248643109806686, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpSource-M4.path b/src/main/deploy/pathplanner/paths/SpSource-M4.path index fd078f0..373ff87 100644 --- a/src/main/deploy/pathplanner/paths/SpSource-M4.path +++ b/src/main/deploy/pathplanner/paths/SpSource-M4.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -21,7 +21,7 @@ }, "prevControl": { "x": 5.7684311921777, - "y": 0.1327818318699705 + "y": 0.13278183186997028 }, "nextControl": null, "isLocked": false, @@ -30,10 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Set Intake Out", "waypointRelativePos": 0.6, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -51,6 +53,7 @@ { "name": "Start Intake Note", "waypointRelativePos": 0.95, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -67,21 +70,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 13.240519915187384, - "rotateFast": true + "rotation": 13.240519915187384 }, "reversed": false, - "folder": "Source Side Auto Paths", - "previewStartingState": { - "rotation": -59.62381673455767, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpSource-M5.path b/src/main/deploy/pathplanner/paths/SpSource-M5.path index 000afea..f17644d 100644 --- a/src/main/deploy/pathplanner/paths/SpSource-M5.path +++ b/src/main/deploy/pathplanner/paths/SpSource-M5.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.25038271698165, - "y": 2.634740404615945 + "y": 2.6347404046159446 }, "isLocked": false, "linkedName": "Speaker Source-Side" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 4.988386913710764, - "y": 2.0338346983000273 + "y": 2.0338346983000277 }, "nextControl": null, "isLocked": false, @@ -38,14 +38,18 @@ "maxVelocity": 3.0, "maxAcceleration": 4.0, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false } } ], + "pointTowardsZones": [], "eventMarkers": [ { "name": "Set Intake Out", "waypointRelativePos": 0.6, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -63,6 +67,7 @@ { "name": "Intake Note", "waypointRelativePos": 0.95, + "endWaypointRelativePos": null, "command": { "type": "sequential", "data": { @@ -79,21 +84,22 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 540.0 + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -15.018360631150598, - "rotateFast": true + "rotation": -15.018360631150598 }, "reversed": false, - "folder": "Source Side Auto Paths", - "previewStartingState": { - "rotation": -60.326805543460246, - "velocity": 0 + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/line 1m.path b/src/main/deploy/pathplanner/paths/line 1m.path index 1e03f98..fd6b196 100644 --- a/src/main/deploy/pathplanner/paths/line 1m.path +++ b/src/main/deploy/pathplanner/paths/line 1m.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 3.0347874519281977, + "x": 3.25, "y": 5.23 }, "isLocked": false, @@ -20,7 +20,7 @@ "y": 6.23 }, "prevControl": { - "x": 2.982072629126527, + "x": 2.75, "y": 6.23 }, "nextControl": null, @@ -30,20 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 1.0, "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 180.0 + "maxAngularAcceleration": 180.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 90.67209552796092, - "rotateFast": false + "rotation": 90.67209552796092 }, "reversed": false, "folder": "Test Autos", - "previewStartingState": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..d48345a --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotWheelbase": 0.546, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index fb177ba..60ea6fa 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -32,13 +32,14 @@ public class Container { private PrimeXboxController m_driverController; private PrimeXboxController m_operatorController; - @Logged(importance = Importance.CRITICAL) + @Logged(name="Vision", importance = Importance.CRITICAL) public VisionSubsystem Vision; - @Logged(importance = Importance.CRITICAL) + @Logged(name="Drive", importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; public Shooter Shooter; public Intake Intake; public Climbers Climbers; + // public PwmLEDs LEDs; public PwmLEDs LEDs; public Compressor Compressor; @@ -51,16 +52,18 @@ public Container(boolean isReal) { m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); // Create new subsystems - LEDs = new PwmLEDs(); + LEDs = new PwmLEDs(isReal); Vision = new VisionSubsystem(new String[] { DriveMap.LimelightFrontName, DriveMap.LimelightRearName }); Drivetrain = new DrivetrainSubsystem(isReal, - LEDs::restorePersistentStripPattern, - LEDs::setStripTemporaryPattern, + LEDs::clearForegroundPattern, + LEDs::setForegroundPattern, Vision::getAllLimelightInputs); - Shooter = new Shooter(LEDs); + Shooter = new Shooter( + LEDs::clearForegroundPattern, + LEDs::setForegroundPattern); Intake = new Intake(); Climbers = new Climbers(); Compressor = new Compressor(30, PneumaticsModuleType.REVPH); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4cf5a8c..dfef840 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,18 +9,17 @@ import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import prime.control.LEDs.Color; -import prime.control.LEDs.Patterns.BlinkPattern; -import prime.control.LEDs.Patterns.ChasePattern; -import prime.control.LEDs.Patterns.PulsePattern; @Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { @@ -58,7 +57,10 @@ public Robot() { @Override public void disabledInit() { - m_robotContainer.LEDs.setStripPersistentPattern(new PulsePattern(onRedAlliance() ? Color.RED : Color.BLUE, 2)); + var disabledPattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) + .breathe(Units.Seconds.of(2.0)); + m_robotContainer.LEDs.setBackgroundPattern(disabledPattern); + m_robotContainer.LEDs.clearForegroundPattern(); } /** @@ -77,7 +79,10 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - m_robotContainer.LEDs.setStripPersistentPattern(new BlinkPattern(onRedAlliance() ? Color.RED : Color.BLUE, 0.250)); + var autoPattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) + .blink(Units.Seconds.of(0.25)); + m_robotContainer.LEDs.setBackgroundPattern(autoPattern); + m_robotContainer.LEDs.clearForegroundPattern(); // Cancel any auto command that's still running and reset the subsystem states if (m_autonomousCommand != null) { @@ -122,9 +127,10 @@ public void teleopInit() { } // Set teleop LED pattern - m_robotContainer.LEDs.setStripPersistentPattern( - new ChasePattern(onRedAlliance() ? Color.RED : Color.BLUE, 0.5, false) - ); + var telePattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) + .scrollAtRelativeSpeed(Units.Hertz.of(2)); + m_robotContainer.LEDs.setBackgroundPattern(telePattern); + m_robotContainer.LEDs.clearForegroundPattern(); m_robotContainer.Drivetrain.EstimatePoseUsingFrontCamera = false; m_robotContainer.Drivetrain.EstimatePoseUsingRearCamera = false; diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index 0bdf342..61ce22d 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -1,91 +1,126 @@ package frc.robot.subsystems; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import prime.control.LEDs.Patterns.LEDPattern; public class PwmLEDs extends SubsystemBase { + public static class VMap { - public static class VMap { + public static final int PwmPort = 9; + public static final int PixelsPerStrip = 78; + public static final double BackgroundDimAmount = 0.5; + } - public static final int PwmPort = 9; - public static final int PixelsPerStrip = 78; - } + private boolean m_isRobotReal; + private AddressableLED m_led; + private AddressableLEDBuffer m_ledBuffer; + private byte _loopErrorCounter = 0; + + private ScheduledExecutorService _updateLoopExecutor; + private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGray); + private LEDPattern m_foregroundPattern = null; + + private Alert m_loopStoppedAlert; + private Dimensionless m_backgroundDimAmount = Units.Percent.of(50); + + public PwmLEDs(boolean isReal) { + m_isRobotReal = isReal; + if (m_isRobotReal) { + // Initialize the LED strip and buffer + m_ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); + m_led = new AddressableLED(VMap.PwmPort); + m_led.setLength(m_ledBuffer.getLength()); + m_led.start(); + + // Apply a default pattern to the LED strip + m_backgroundPattern.applyTo(m_ledBuffer); + + // Setup the warning for when the loop stops + m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); + m_loopStoppedAlert.set(false); + + // _updateLoopExecutor = Executors.newScheduledThreadPool(1); + // // Start the pattern update loop at 200Hz with a 3ms delay to running at the same time as periodic functions + // _updateLoopExecutor.scheduleAtFixedRate(this::updateLedStrip, 3, 5, java.util.concurrent.TimeUnit.MILLISECONDS); + } else { + // initialize nothing + } + } - private AddressableLED _led; - private AddressableLEDBuffer _ledBuffer; + @Override + public void periodic() { + if (!m_isRobotReal) + return; - private final ScheduledExecutorService _updateLoopExecutor = Executors.newScheduledThreadPool(1); - private LEDPattern _persistentPattern; - private LEDPattern _temporaryPattern; + /** + * TODO: Test at 20ms periodic update loop, then test with ScheduledExecutorService. + * Determine if the pattern velocity is thrown off, or is simply rendered more smoothly. + */ + updateLedStrip(); + } - public PwmLEDs() { - // Initialize the LED strip and buffer - _ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); - _led = new AddressableLED(VMap.PwmPort); - _led.setLength(_ledBuffer.getLength()); + public Command setBackgroundPattern(LEDPattern backgroundPattern) { + if (!m_isRobotReal) + return runOnce(null); - // Set the strip to a default color and start the LED strip - for (var i = 0; i < _ledBuffer.getLength(); i++) { - _ledBuffer.setRGB(i, 100, 100, 100); + return runOnce(() -> { + m_backgroundPattern = backgroundPattern; + }); } - _led.setData(_ledBuffer); - _led.start(); - - // Start the pattern update loop at 142hz with a default pattern - _updateLoopExecutor.scheduleAtFixedRate(this::ledUpdateLoop, 0, 7, java.util.concurrent.TimeUnit.MILLISECONDS); - } - - /** - * Set the persistent pattern of the LED strip - */ - public void setStripPersistentPattern(LEDPattern pattern) { - _persistentPattern = pattern; - } - - /** - * Set the temporary pattern of the LED strip - */ - public void setStripTemporaryPattern(LEDPattern pattern) { - _temporaryPattern = pattern; - } - - /** - * Set the LED strip back to it's persistent pattern - */ - public void restorePersistentStripPattern() { - _temporaryPattern = null; - } - - private byte _loopErrorCounter = 0; - - private void ledUpdateLoop() { - try { - // If the temporary pattern for this section is not null, use it instead of the persistent pattern - var pattern = _temporaryPattern != null ? _temporaryPattern : _persistentPattern; - - // If the pattern is not null, update the LED strip - if (pattern != null) { - // Request for the pattern to calculate the next frame and update the buffer - pattern.updateBuffer(0, VMap.PixelsPerStrip, _ledBuffer); - - // Update the LED strip with the new buffer - _led.setData(_ledBuffer); - } - } catch (Exception e) { - _loopErrorCounter++; - DriverStation.reportError("[LEDs:ERROR] Error in update loop: " + e.getMessage(), e.getStackTrace()); - - if (_loopErrorCounter > 3) { - var msg = "[LEDs:ERROR] LED update loop has failed 3 times. Stopping loop."; - DriverStation.reportError(msg, false); - System.out.println(msg); - _updateLoopExecutor.shutdown(); - } + + public Command setForegroundPattern(LEDPattern foregroundPattern) { + if (!m_isRobotReal) + return runOnce(null); + + return runOnce(() -> { + m_foregroundPattern = foregroundPattern; + }); + } + + public Command clearForegroundPattern() { + if (!m_isRobotReal) + return runOnce(null); + + return runOnce(() -> { + m_foregroundPattern = null; + }); + } + + private void updateLedStrip() { + if (!m_isRobotReal || _loopErrorCounter > 3) + return; + + try { + if (m_foregroundPattern == null) { + m_backgroundPattern.applyTo(m_ledBuffer); + } else { + var dimmedBackground = m_backgroundPattern.atBrightness(m_backgroundDimAmount); + m_foregroundPattern.overlayOn(dimmedBackground).applyTo(m_ledBuffer); + } + + m_led.setData(m_ledBuffer); + } catch (Exception e) { + _loopErrorCounter++; + DriverStation.reportError("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage(), e.getStackTrace()); + + if (_loopErrorCounter > 3) { + var msg = "[LEDs:ERROR] LED update loop has failed 3 times. Stopping loop."; + DriverStation.reportError(msg, false); + System.out.println(msg); + m_loopStoppedAlert.set(true); + _updateLoopExecutor.shutdown(); + } + } } - } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 14a5793..ed9f3d4 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -8,19 +8,20 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import java.util.Map; -import prime.control.LEDs.Color; -import prime.control.LEDs.Patterns.BlinkPattern; -import prime.control.LEDs.Patterns.ChasePattern; -import prime.control.LEDs.Patterns.SolidPattern; +import java.util.function.Consumer; public class Shooter extends SubsystemBase { @@ -35,20 +36,29 @@ public static class VMap { public static final int ElevationSolenoidReverseChannel = 7; } - private PwmLEDs m_leds; private TalonFX m_talonFX; private VictorSPX m_victorSPX; private DoubleSolenoid m_elevationSolenoid; private DigitalInput m_noteDetector; + private Runnable m_clearForegroundPatternFunc; + private Consumer m_setForegroundPatternFunc; + private LEDPattern m_elevatorUpLEDPattern = LEDPattern.solid(Color.kWhite); + private LEDPattern m_shootingLEDPattern = LEDPattern.steps(Map.of(0.0, Color.kGreen, 0.10, Color.kBlack)) + .scrollAtRelativeSpeed(Units.Hertz.of(4)); + private LEDPattern m_noteDetectedLEDPattern = LEDPattern.solid(Color.kOrange) + .blink(Units.Seconds.of(0.2)); + // #endregion /** * Creates a new Shooter with a given configuration * @param config */ - public Shooter(PwmLEDs leds) { - m_leds = leds; + public Shooter( + Runnable restoreLEDPersistentPatternFunc, + Consumer setLEDTemporaryPatternFunc + ) { setName("Shooter"); m_talonFX = new TalonFX(VMap.TalonFXCanID); @@ -70,6 +80,9 @@ public Shooter(PwmLEDs leds) { ); m_noteDetector = new DigitalInput(VMap.NoteDetectorDIOChannel); + + m_clearForegroundPatternFunc = restoreLEDPersistentPatternFunc; + m_setForegroundPatternFunc = setLEDTemporaryPatternFunc; } //#region Control Methods @@ -93,7 +106,7 @@ public void runGreenWheel(double speed) { public void stopMotors() { m_talonFX.stopMotor(); m_victorSPX.set(VictorSPXControlMode.PercentOutput, 0); - m_leds.restorePersistentStripPattern(); + m_clearForegroundPatternFunc.run(); } /** @@ -110,12 +123,12 @@ public void setElevator(Value value) { public void setElevatorUp() { setElevator(Value.kForward); - m_leds.setStripTemporaryPattern(new SolidPattern(Color.WHITE)); + m_setForegroundPatternFunc.accept(m_elevatorUpLEDPattern); } public void setElevatorDown() { setElevator(Value.kReverse); - m_leds.restorePersistentStripPattern(); + m_clearForegroundPatternFunc.run(); } //#endregion @@ -127,9 +140,9 @@ public void periodic() { var newNoteDetectedValue = isNoteLoaded(); if (newNoteDetectedValue != m_lastNoteDetectedValue) { if (newNoteDetectedValue && !m_lastNoteDetectedValue) { - m_leds.setStripTemporaryPattern(new BlinkPattern(prime.control.LEDs.Color.ORANGE, 0.2)); + m_setForegroundPatternFunc.accept(m_noteDetectedLEDPattern); } else { - m_leds.restorePersistentStripPattern(); + m_clearForegroundPatternFunc.run(); } // Save the new value @@ -168,7 +181,7 @@ public Command scoreInAmpCommand() { public Command startShootingNoteCommand() { return Commands.runOnce(() -> { runShooter(1); - m_leds.setStripTemporaryPattern(new ChasePattern(Color.GREEN, 0.25, isNoteLoaded())); + m_setForegroundPatternFunc.accept(m_shootingLEDPattern); }); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java index 15f5ad9..1945ff8 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java @@ -11,7 +11,7 @@ public class DrivetrainIOInputs { public double GyroAccelX = 0; public double GyroAccelY = 0; public double GyroAccelZ = 0; - public boolean SnapOnTarget = false; + public boolean SnapIsOnTarget = false; public ChassisSpeeds RobotRelativeChassisSpeeds = new ChassisSpeeds(); public Pose2d EstimatedRobotPose = new Pose2d(); public double SnapCorrectionRadiansPerSecond = 0; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java index c8c6ffb..5279c84 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java @@ -174,7 +174,7 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snap desiredChassisSpeeds.omegaRadiansPerSecond = snapCorrection; // Report back if snap is on-target - m_inputs.SnapOnTarget = Math.abs(desiredChassisSpeeds.omegaRadiansPerSecond) < 0.1; + m_inputs.SnapIsOnTarget = Math.abs(desiredChassisSpeeds.omegaRadiansPerSecond) < 0.1; } // Correct drift by taking the input speeds and converting them to a desired per-period speed. This is known as "discretizing" diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index 5bf88b4..8bf3359 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -30,22 +30,22 @@ public class DrivetrainIOSim implements IDrivetrainIO { private SwerveDrivePoseEstimator m_poseEstimator; @Logged(name = "FrontLeftInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_frontLeftInputs; + private SwerveModuleIOInputs m_frontLeftInputs = new SwerveModuleIOInputs(); @Logged(name = "FrontRightInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_frontRightInputs; + private SwerveModuleIOInputs m_frontRightInputs = new SwerveModuleIOInputs(); @Logged(name = "RearLeftInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_rearLeftInputs; + private SwerveModuleIOInputs m_rearLeftInputs = new SwerveModuleIOInputs(); @Logged(name = "RearRightInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_rearRightInputs; + private SwerveModuleIOInputs m_rearRightInputs = new SwerveModuleIOInputs(); @Logged(name = "FrontLeftOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_frontLeftOutputs; + private SwerveModuleIOOutputs m_frontLeftOutputs = new SwerveModuleIOOutputs(); @Logged(name = "FrontRightOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_frontRightOutputs; + private SwerveModuleIOOutputs m_frontRightOutputs = new SwerveModuleIOOutputs(); @Logged(name = "RearLeftOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_rearLeftOutputs; + private SwerveModuleIOOutputs m_rearLeftOutputs = new SwerveModuleIOOutputs(); @Logged(name = "RearRightOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_rearRightOutputs; + private SwerveModuleIOOutputs m_rearRightOutputs = new SwerveModuleIOOutputs(); public DrivetrainIOSim() { m_inputs = new DrivetrainIOInputs(); @@ -149,7 +149,7 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snap // If snap-to is enabled, calculate and override the input rotational speed to reach the setpoint if (snapAngleEnabled) { // Report back that snap is on-target since we're assuming there is no loss in the simulation - m_inputs.SnapOnTarget = true; + m_inputs.SnapIsOnTarget = true; } // Correct drift by taking the input speeds and converting them to a desired per-period speed. This is known as "discretizing" diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 60d6e6d..280ec79 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -11,10 +11,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -28,17 +31,13 @@ import java.util.function.Supplier; import prime.control.SwerveControlSuppliers; -import prime.control.LEDs.Color; -import prime.control.LEDs.Patterns.LEDPattern; -import prime.control.LEDs.Patterns.PulsePattern; -import prime.control.LEDs.Patterns.SolidPattern; import prime.vision.LimelightInputs; @Logged(strategy = Strategy.OPT_IN) public class DrivetrainSubsystem extends SubsystemBase { - private Runnable m_restoreLEDPersistentPatternFunc; - private Consumer m_setLEDTemporaryPatternFunc; + private Runnable m_clearForegroundPatternFunc; + private Consumer m_setForegroundPatternFunc; // Shuffleboard Drivetrain tab configuration private ShuffleboardTab d_drivetrainTab = Shuffleboard.getTab("Drivetrain"); @@ -73,18 +72,22 @@ public class DrivetrainSubsystem extends SubsystemBase { @Logged(importance = Logged.Importance.CRITICAL) public boolean WithinPoseEstimationVelocity = true; + private LEDPattern m_snapOnTargetPattern = LEDPattern.solid(Color.kGreen); + private LEDPattern m_snapOffTargetPattern = LEDPattern.steps(Map.of(0.0, Color.kRed, 0.25, Color.kBlack)) + .scrollAtRelativeSpeed(Units.Hertz.of(2)); + /** * Creates a new Drivetrain. */ public DrivetrainSubsystem( boolean isReal, - Runnable restoreLEDPersistentPatternFunc, - Consumer setLEDTemporaryPatternFunc, + Runnable clearForegroundPatternFunc, + Consumer setForegroundPatternFunc, Supplier limelightInputsSupplier) { setName("Drivetrain"); - m_restoreLEDPersistentPatternFunc = restoreLEDPersistentPatternFunc; - m_setLEDTemporaryPatternFunc = setLEDTemporaryPatternFunc; + m_clearForegroundPatternFunc = clearForegroundPatternFunc; + m_setForegroundPatternFunc = setForegroundPatternFunc; // Create IO m_driveio = isReal @@ -165,7 +168,7 @@ private void drivePathPlanner(ChassisSpeeds pathSpeeds) { */ private void setSnapToEnabled(boolean enabled) { m_outputs.SnapEnabled = enabled; - if (!enabled) m_restoreLEDPersistentPatternFunc.run(); + if (!enabled) m_clearForegroundPatternFunc.run(); } /** @@ -217,10 +220,10 @@ public void periodic() { if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); // Update LEDs - if (m_inputs.SnapOnTarget) { - m_setLEDTemporaryPatternFunc.accept(new SolidPattern(Color.GREEN)); + if (m_inputs.SnapIsOnTarget) { + m_setForegroundPatternFunc.accept(m_snapOnTargetPattern); } else { - m_setLEDTemporaryPatternFunc.accept(new PulsePattern(Color.RED, 0.5)); + m_setForegroundPatternFunc.accept(m_snapOffTargetPattern); } // Send outputs to the drive IO @@ -244,7 +247,7 @@ public Command defaultDriveCommand(SwerveControlSuppliers controlSuppliers) { // If the driver is trying to rotate the robot, disable snap-to control if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { setSnapToEnabled(false); - m_restoreLEDPersistentPatternFunc.run(); + m_clearForegroundPatternFunc.run(); } // Convert inputs to MPS diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index 3bcba7c..c132a90 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -2,7 +2,7 @@ public class SwerveModuleIOSim implements ISwerveModuleIO { - private SwerveModuleIOInputs m_inputs; + private SwerveModuleIOInputs m_inputs = new SwerveModuleIOInputs(); public SwerveModuleIOSim(SwerveModuleConfig moduleMap) { } diff --git a/src/main/java/prime/control/LEDs/Color.java b/src/main/java/prime/control/LEDs/Color.java deleted file mode 100644 index 84f71f2..0000000 --- a/src/main/java/prime/control/LEDs/Color.java +++ /dev/null @@ -1,64 +0,0 @@ -package prime.control.LEDs; - -import edu.wpi.first.wpilibj.DriverStation; - -public class Color { - - public int r; - public int g; - public int b; - - public Color(int r, int g, int b) { - if (r < 0 || g < 0 || b < 0 || r > 255 || g > 255 || b > 255) { - DriverStation.reportError("Invalid color", false); - return; - } - - this.r = r; - this.g = g; - this.b = b; - } - - // Predefined static colors - public static final Color OFF = new Color(0, 0, 0); - - public static final Color RED = new Color(255, 0, 0); - - public static final Color GREEN = new Color(0, 255, 0); - - public static final Color BLUE = new Color(0, 0, 255); - - public static final Color WHITE = new Color(255, 255, 255); - - public static final Color YELLOW = new Color(255, 255, 0); - - public static final Color CYAN = new Color(0, 255, 255); - - public static final Color MAGENTA = new Color(255, 0, 255); - - public static final Color ORANGE = new Color(255, 165, 0); - - public static final Color PINK = new Color(255, 192, 203); - - public static final Color PURPLE = new Color(128, 0, 128); - - public static final Color LIME = new Color(0, 255, 0); - - public static final Color TEAL = new Color(0, 128, 128); - - public static final Color AQUA = new Color(0, 255, 255); - - public static final Color VIOLET = new Color(238, 130, 238); - - public static final Color BROWN = new Color(165, 42, 42); - - public static final Color TAN = new Color(210, 180, 140); - - public static final Color BEIGE = new Color(245, 245, 220); - - public static final Color MAROON = new Color(128, 0, 0); - - public static final Color OLIVE = new Color(128, 128, 0); - - public static final Color NAVY = new Color(0, 0, 128); -} diff --git a/src/main/java/prime/control/LEDs/LEDEffect.java b/src/main/java/prime/control/LEDs/LEDEffect.java deleted file mode 100644 index 4ae1fb5..0000000 --- a/src/main/java/prime/control/LEDs/LEDEffect.java +++ /dev/null @@ -1,8 +0,0 @@ -package prime.control.LEDs; - -public enum LEDEffect { - Solid, - Blink, - Chase, - Pulse, -} diff --git a/src/main/java/prime/control/LEDs/Patterns/BlinkPattern.java b/src/main/java/prime/control/LEDs/Patterns/BlinkPattern.java deleted file mode 100644 index c6ef88e..0000000 --- a/src/main/java/prime/control/LEDs/Patterns/BlinkPattern.java +++ /dev/null @@ -1,34 +0,0 @@ -package prime.control.LEDs.Patterns; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import prime.control.LEDs.Color; -import prime.control.LEDs.LEDEffect; - -public class BlinkPattern extends LEDPattern { - - private static final double FRAME_COUNT = 2; - - /** - * Create a new BlinkPattern with a color and speed - * @param color The color of the pattern - * @param speed The speed of the pattern in seconds per iteration - */ - public BlinkPattern(Color color, double speed) { - super(color, LEDEffect.Blink, speed / FRAME_COUNT, false); - } - - @Override - public void updateBuffer(int startingIndex, int length, AddressableLEDBuffer buffer) { - if (isUpdatable()) { - Frame = Frame == 0 ? 1 : 0; - - var color = Frame == 0 ? Color : prime.control.LEDs.Color.OFF; - - for (int i = 0; i < length; i++) { - buffer.setRGB(startingIndex + i, color.r, color.g, color.b); - } - - LastFrameTime = System.currentTimeMillis(); - } - } -} diff --git a/src/main/java/prime/control/LEDs/Patterns/ChasePattern.java b/src/main/java/prime/control/LEDs/Patterns/ChasePattern.java deleted file mode 100644 index 229377d..0000000 --- a/src/main/java/prime/control/LEDs/Patterns/ChasePattern.java +++ /dev/null @@ -1,97 +0,0 @@ -package prime.control.LEDs.Patterns; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import prime.control.LEDs.LEDEffect; - -public class ChasePattern extends LEDPattern { - - private static final int CHASE_LENGTH = 8; // Length of the chasing group - private static final int FADE_LENGTH = 16; // Length of the fading tail - - /** - * Create a new ChasePattern with a color and speed - * - * @param color The color of the pattern - * @param chaseSpeedSeconds The speed of the pattern in seconds per iteration - */ - public ChasePattern(prime.control.LEDs.Color color, double chaseSpeedSeconds, boolean reversed) { - super(color, LEDEffect.Chase, chaseSpeedSeconds, reversed); - } - - @Override - public void updateBuffer(int startingIndex, int length, AddressableLEDBuffer buffer) { - var totalFrameCount = length + CHASE_LENGTH + FADE_LENGTH; - var frameSpeedS = EffectSpeedSeconds / totalFrameCount; - var currentTime = System.currentTimeMillis(); - - if (currentTime - LastFrameTime >= (frameSpeedS * 1000)) { - if (!Reversed) { - // Set the chase group - var chaseGroupFirst = startingIndex + Frame; - var chaseGroupLast = chaseGroupFirst - CHASE_LENGTH; - - for (int i = startingIndex; i < (startingIndex + length); i++) { - // If i is a chase pixel, set it to ON - if (i > chaseGroupLast && i <= chaseGroupFirst) { - buffer.setRGB((int) i, Color.r, Color.g, Color.b); - } - - // If it's within FADE_LENGTH of the last chase pixel, fade it out - var lastFade = chaseGroupLast - FADE_LENGTH; - if (i <= chaseGroupLast && i > lastFade) { - var distanceFromLastChase = Math.abs((chaseGroupLast + 1) - i); - double brightnessScalar = (1d / (double) FADE_LENGTH) * (double) (FADE_LENGTH - distanceFromLastChase); - - var scaledColor = new prime.control.LEDs.Color( - (int) (Color.r * brightnessScalar), - (int) (Color.g * brightnessScalar), - (int) (Color.b * brightnessScalar) - ); - - buffer.setRGB(i, scaledColor.r, scaledColor.g, scaledColor.b); - } - - if (i <= lastFade) { - buffer.setRGB(i, 0, 0, 0); - } - } - - Frame = Frame + 1; - if (Frame >= totalFrameCount) { - Frame = 0; - } - } else { - // Set the chase group - var chaseGroupFirst = startingIndex + length - Frame; - var chaseGroupLast = chaseGroupFirst + CHASE_LENGTH; - for (long i = chaseGroupLast; i >= chaseGroupFirst; i--) { - if (i >= startingIndex && i < startingIndex + length) { - buffer.setRGB((int) i, Color.r, Color.g, Color.b); - } - } - - // Set the fading trail - var fadeTrailFirst = chaseGroupLast + 1; - var fadeTrailLast = fadeTrailFirst + FADE_LENGTH; - for (long i = fadeTrailLast; i >= fadeTrailFirst; i--) { - if (i >= startingIndex && i < startingIndex + length) { - var brightnessScalar = (float) (length - i) / FADE_LENGTH; - buffer.setRGB( - (int) i, - (byte) (Color.r * brightnessScalar), - (byte) (Color.g * brightnessScalar), - (byte) (Color.b * brightnessScalar) - ); - } - } - - Frame = Frame - 1; - if (Frame <= 0) { - Frame = totalFrameCount; - } - } - - LastFrameTime = System.currentTimeMillis(); - } - } -} diff --git a/src/main/java/prime/control/LEDs/Patterns/LEDPattern.java b/src/main/java/prime/control/LEDs/Patterns/LEDPattern.java deleted file mode 100644 index 3b52c37..0000000 --- a/src/main/java/prime/control/LEDs/Patterns/LEDPattern.java +++ /dev/null @@ -1,90 +0,0 @@ -package prime.control.LEDs.Patterns; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import prime.control.LEDs.Color; -import prime.control.LEDs.LEDEffect; - -public abstract class LEDPattern { - - protected static final double MIN_FRAME_SPEED = 0.007; - - /** - * The color of the pattern - */ - public Color Color; - - /** - * The effect of the pattern - */ - public LEDEffect Effect; - - /** - * The speed of the pattern in seconds per iteration - */ - public double EffectSpeedSeconds; - - /** - * Whether the pattern is reversed. This is sometimes controlled by the pattern itself - */ - public boolean Reversed; - - // State variables - protected int Frame = 0; - protected long LastFrameTime = 0; - - public LEDPattern(int r, int g, int b, LEDEffect effect, double effectSpeedSeconds, boolean reversed) { - this.Color = new Color((byte) r, (byte) g, (byte) b); - this.Effect = effect; - this.Reversed = reversed; - - if (effectSpeedSeconds < MIN_FRAME_SPEED) { - System.out.println("Speed for LED effect " + effect.name() + " is too fast, setting to minimum speed of 7ms"); - this.EffectSpeedSeconds = MIN_FRAME_SPEED; - } else { - this.EffectSpeedSeconds = effectSpeedSeconds; - } - } - - public LEDPattern(Color color, LEDEffect effect, double effectSpeedSeconds, boolean reversed) { - this.Color = color; - this.Effect = effect; - this.Reversed = reversed; - - if (effectSpeedSeconds < MIN_FRAME_SPEED) { - System.out.println("Speed for LED effect " + effect.name() + " is too fast, setting to minimum speed of 7ms"); - this.EffectSpeedSeconds = MIN_FRAME_SPEED; - } else { - this.EffectSpeedSeconds = effectSpeedSeconds; - } - } - - /** - * Update the buffer with the pattern - * @param startingIndex The starting index of the buffer to update - * @param length The length of the buffer to update - * @param buffer The buffer to update - */ - public abstract void updateBuffer(int startingIndex, int length, AddressableLEDBuffer buffer); - - /** - * Check if the pattern is the same as another pattern - * @param other The other pattern to compare to - */ - public boolean isSameAs(LEDPattern other) { - return ( - this.Color.equals(other.Color) && - this.Effect == other.Effect && - this.EffectSpeedSeconds == other.EffectSpeedSeconds && - this.Reversed == other.Reversed - ); - } - - /** - * Check if the pattern is updatable based on the speed - */ - protected boolean isUpdatable() { - var currentTime = System.currentTimeMillis(); - - return currentTime - LastFrameTime >= (EffectSpeedSeconds * 1000); - } -} diff --git a/src/main/java/prime/control/LEDs/Patterns/PulsePattern.java b/src/main/java/prime/control/LEDs/Patterns/PulsePattern.java deleted file mode 100644 index 28a0280..0000000 --- a/src/main/java/prime/control/LEDs/Patterns/PulsePattern.java +++ /dev/null @@ -1,68 +0,0 @@ -package prime.control.LEDs.Patterns; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import prime.control.LEDs.LEDEffect; - -public class PulsePattern extends LEDPattern { - - private static final int MIN_FRAME_COUNT = 70; - private int _frameCount = MIN_FRAME_COUNT; - - /** - * Create a new PulsePattern with a color and speed - * @param color The color of the pattern - * @param pulseSpeedSeconds The speed of the pattern in seconds per iteration - */ - public PulsePattern(prime.control.LEDs.Color color, double pulseSpeedSeconds) { - // Get frame count from pulseSpeedSeconds and minimum frame speed - super(color, LEDEffect.Pulse, getFastestFrameSpeed(pulseSpeedSeconds), false); - _frameCount = getMaxFrameCount(pulseSpeedSeconds); - } - - /** - * Get the fastest frame speed for the given pulse speed - */ - private static double getFastestFrameSpeed(double pulseSpeedSeconds) { - return pulseSpeedSeconds / getMaxFrameCount(pulseSpeedSeconds); - } - - /** - * Get the maximum frame count for the given pulse speed - */ - private static int getMaxFrameCount(double pulseSpeedSeconds) { - return Math.max((int) (pulseSpeedSeconds / MIN_FRAME_SPEED), MIN_FRAME_COUNT); - } - - @Override - public void updateBuffer(int startingIndex, int length, AddressableLEDBuffer buffer) { - if (isUpdatable()) { - // calculate brightness of the color by the current Frame. Frame 0 is OFF, frame FRAME_COUNT is MAX_BRIGHTNESS - var scalar = (float) Frame / _frameCount; - var color = new prime.control.LEDs.Color( - (int) (Color.r * scalar), - (int) (Color.g * scalar), - (int) (Color.b * scalar) - ); - - // Set the color of each pixel in the buffer length - for (int i = 0; i < length; i++) { - buffer.setRGB(startingIndex + i, color.r, color.g, color.b); - } - - // Increment the frame forward if not reversed, decrement if reversed - Frame += !Reversed ? 4 : -4; - - if (Frame >= _frameCount) { - // If the frame is at the end, set to reverse direction - Reversed = true; - Frame = _frameCount; - } else if (Frame <= 0) { - // If the frame is at the start, set to forward direction - Reversed = false; - Frame = 0; - } - - LastFrameTime = System.currentTimeMillis(); - } - } -} diff --git a/src/main/java/prime/control/LEDs/Patterns/SolidPattern.java b/src/main/java/prime/control/LEDs/Patterns/SolidPattern.java deleted file mode 100644 index ed33e03..0000000 --- a/src/main/java/prime/control/LEDs/Patterns/SolidPattern.java +++ /dev/null @@ -1,32 +0,0 @@ -package prime.control.LEDs.Patterns; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import prime.control.LEDs.LEDEffect; - -public class SolidPattern extends LEDPattern { - - /** - * Create a new SolidPattern with a color - * @param color The color of the pattern - */ - public SolidPattern(prime.control.LEDs.Color color) { - super(color, LEDEffect.Solid, 0, false); - } - - /** - * Create a new SolidPattern with an RGB color - * @param r The red value of the color - * @param g The green value of the color - * @param b The blue value of the color - */ - public SolidPattern(int r, int g, int b) { - super(r, g, b, LEDEffect.Solid, 0, false); - } - - @Override - public void updateBuffer(int startingIndex, int length, AddressableLEDBuffer buffer) { - for (int i = 0; i < length; i++) { - buffer.setRGB(startingIndex + i, Color.r, Color.g, Color.b); - } - } -} From a9f3f5618b482be59f233c4641c62d2c464a5add Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Wed, 30 Oct 2024 13:54:21 -0500 Subject: [PATCH 07/24] fixed some issues that were causing simulation to fail prematurely --- src/main/java/frc/robot/Container.java | 108 +++++++++--------- src/main/java/frc/robot/Robot.java | 14 +-- .../java/frc/robot/subsystems/PwmLEDs.java | 6 +- .../robot/subsystems/vision/LimeLightNT.java | 2 +- 4 files changed, 64 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index 60ea6fa..d39a4f0 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -36,12 +36,12 @@ public class Container { public VisionSubsystem Vision; @Logged(name="Drive", importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; - public Shooter Shooter; - public Intake Intake; - public Climbers Climbers; + // public Shooter Shooter; + // public Intake Intake; + // public Climbers Climbers; // public PwmLEDs LEDs; public PwmLEDs LEDs; - public Compressor Compressor; + // public Compressor Compressor; private CombinedCommands m_combinedCommands; @@ -61,21 +61,21 @@ public Container(boolean isReal) { LEDs::clearForegroundPattern, LEDs::setForegroundPattern, Vision::getAllLimelightInputs); - Shooter = new Shooter( - LEDs::clearForegroundPattern, - LEDs::setForegroundPattern); - Intake = new Intake(); - Climbers = new Climbers(); - Compressor = new Compressor(30, PneumaticsModuleType.REVPH); - Compressor.enableDigital(); + // Shooter = new Shooter( + // LEDs::clearForegroundPattern, + // LEDs::setForegroundPattern); + // Intake = new Intake(); + // Climbers = new Climbers(); + // Compressor = new Compressor(30, PneumaticsModuleType.REVPH); + // Compressor.enableDigital(); m_combinedCommands = new CombinedCommands(); // Register the named commands from each subsystem that may be used in PathPlanner NamedCommands.registerCommands(Drivetrain.getNamedCommands()); - NamedCommands.registerCommands(Intake.getNamedCommands()); - NamedCommands.registerCommands(Shooter.getNamedCommands()); - NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // Register the combined named commands that use multiple subsystems + // NamedCommands.registerCommands(Intake.getNamedCommands()); + // NamedCommands.registerCommands(Shooter.getNamedCommands()); + // NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // Register the combined named commands that use multiple subsystems // Create Auto chooser and Auto tab in Shuffleboard configAutonomousDashboardItems(); @@ -138,16 +138,16 @@ public void configureDriverControls() { m_driverController.pov(Controls.right).onTrue(Drivetrain.setSnapToSetpointCommand(90)); // Climbers - m_driverController.y().onTrue(Climbers.toggleClimbControlsCommand()); - m_driverController.start().onTrue(Climbers.setArmsUpCommand()); - Climbers.setDefaultCommand( - Climbers.defaultClimbingCommand( - m_driverController.button(Controls.RB), - m_driverController.button(Controls.LB), - () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), - () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER) - ) - ); + // m_driverController.y().onTrue(Climbers.toggleClimbControlsCommand()); + // m_driverController.start().onTrue(Climbers.setArmsUpCommand()); + // Climbers.setDefaultCommand( + // Climbers.defaultClimbingCommand( + // m_driverController.button(Controls.RB), + // m_driverController.button(Controls.LB), + // () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), + // () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER) + // ) + // ); } /** @@ -155,35 +155,35 @@ public void configureDriverControls() { */ public void configureOperatorControls() { // Intake ======================================== - m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle in/out - - m_operatorController // When the trigger is pressed, intake a note at a variable speed - .leftTrigger(0.1) - .whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) - .onFalse(Intake.stopRollersCommand()); - - m_operatorController // When the trigger is pressed, eject a note at a constant speed - .rightTrigger(0.1) - .whileTrue(Intake.ejectNoteCommand()) - .onFalse(Intake.stopRollersCommand()); - - // Shooter ======================================== - m_operatorController // Toggle the elevation of the shooter - .rightBumper() - .onTrue(Shooter.toggleElevationCommand()); - - m_operatorController // Runs only the shooter motors at a constant speed to score in the amp - .x() - .whileTrue(Shooter.startShootingNoteCommand()) - .onFalse(Shooter.stopMotorsCommand()); - - // Combined shooter and intake commands =========== - m_operatorController // score in speaker - .b() - .onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); - - m_operatorController // Run sequence to load a note into the shooter for scoring in the amp - .y() - .onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); + // m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle in/out + + // m_operatorController // When the trigger is pressed, intake a note at a variable speed + // .leftTrigger(0.1) + // .whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) + // .onFalse(Intake.stopRollersCommand()); + + // m_operatorController // When the trigger is pressed, eject a note at a constant speed + // .rightTrigger(0.1) + // .whileTrue(Intake.ejectNoteCommand()) + // .onFalse(Intake.stopRollersCommand()); + + // // Shooter ======================================== + // m_operatorController // Toggle the elevation of the shooter + // .rightBumper() + // .onTrue(Shooter.toggleElevationCommand()); + + // m_operatorController // Runs only the shooter motors at a constant speed to score in the amp + // .x() + // .whileTrue(Shooter.startShootingNoteCommand()) + // .onFalse(Shooter.stopMotorsCommand()); + + // // Combined shooter and intake commands =========== + // m_operatorController // score in speaker + // .b() + // .onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); + + // m_operatorController // Run sequence to load a note into the shooter for scoring in the amp + // .y() + // .onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dfef840..29c5576 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,13 +24,11 @@ @Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { - @Logged(name = "Robot/Container", importance = Importance.CRITICAL) + @Logged(name = "Container", importance = Importance.CRITICAL) private Container m_robotContainer; private Command m_autonomousCommand; public Robot() { - super(); - // Configure logging DataLogManager.start(); DataLogManager.logConsoleOutput(true); @@ -89,9 +87,9 @@ public void autonomousInit() { m_autonomousCommand.cancel(); // Stop the shooter and intake motors in case they're still running and set the intake IN - m_robotContainer.Shooter.stopMotorsCommand().schedule(); - m_robotContainer.Intake.stopRollersCommand().schedule(); - m_robotContainer.Intake.setIntakeInCommand().schedule(); + // m_robotContainer.Shooter.stopMotorsCommand().schedule(); + // m_robotContainer.Intake.stopRollersCommand().schedule(); + // m_robotContainer.Intake.setIntakeInCommand().schedule(); } m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -122,8 +120,8 @@ public void teleopInit() { m_autonomousCommand.cancel(); // Stop the shooter and intake motors in case they're still running - m_robotContainer.Shooter.stopMotorsCommand().schedule(); - m_robotContainer.Intake.stopRollersCommand().schedule(); + // m_robotContainer.Shooter.stopMotorsCommand().schedule(); + // m_robotContainer.Intake.stopRollersCommand().schedule(); } // Set teleop LED pattern diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index 61ce22d..b106b79 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -72,7 +72,7 @@ public void periodic() { public Command setBackgroundPattern(LEDPattern backgroundPattern) { if (!m_isRobotReal) - return runOnce(null); + return runOnce(() -> {}); return runOnce(() -> { m_backgroundPattern = backgroundPattern; @@ -81,7 +81,7 @@ public Command setBackgroundPattern(LEDPattern backgroundPattern) { public Command setForegroundPattern(LEDPattern foregroundPattern) { if (!m_isRobotReal) - return runOnce(null); + return runOnce(() -> {}); return runOnce(() -> { m_foregroundPattern = foregroundPattern; @@ -90,7 +90,7 @@ public Command setForegroundPattern(LEDPattern foregroundPattern) { public Command clearForegroundPattern() { if (!m_isRobotReal) - return runOnce(null); + return runOnce(() -> {}); return runOnce(() -> { m_foregroundPattern = null; diff --git a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java index 655b8e5..4c5d8a9 100644 --- a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java +++ b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java @@ -21,7 +21,7 @@ public class LimeLightNT implements AutoCloseable { private NetworkTable m_limelightTable; private ExecutorService m_executorService = Executors.newSingleThreadExecutor(); - private LimelightInputs m_inputs; + private LimelightInputs m_inputs = new LimelightInputs(); public LimeLightNT(NetworkTableInstance instance, String tableName) { m_limelightTable = instance.getTable(tableName); From 100ad6d2986b86f1c6c78ea2c6633efc8b4a1f58 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Thu, 31 Oct 2024 22:56:00 -0500 Subject: [PATCH 08/24] added faster LED updating to Robot.java via addPeriodic for thread safety --- src/main/java/frc/robot/Robot.java | 10 +++ .../java/frc/robot/subsystems/PwmLEDs.java | 78 +++++++------------ 2 files changed, 38 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 29c5576..9313199 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,8 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; public Robot() { + super(0.02); // Run the robot loop at 50Hz + // Configure logging DataLogManager.start(); DataLogManager.logConsoleOutput(true); @@ -51,6 +53,14 @@ public Robot() { // Initialize the robot container m_robotContainer = new Container(isReal()); + + // Schedule the LED patterns to run at 200Hz + // This is the recommended way to schedule a periodic task that runs faster than the robot loop + // This also adds a delay of 3ms to the task to ensure it does not run at the same time as + // subsystem periodic functions + addPeriodic(m_robotContainer.LEDs::updateLedStrip, + Units.Milliseconds.of(5), + Units.Milliseconds.of(3)); } @Override diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index b106b79..c3782a0 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -1,8 +1,5 @@ package frc.robot.subsystems; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; - import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.wpilibj.AddressableLED; @@ -16,7 +13,6 @@ public class PwmLEDs extends SubsystemBase { public static class VMap { - public static final int PwmPort = 9; public static final int PixelsPerStrip = 78; public static final double BackgroundDimAmount = 0.5; @@ -27,7 +23,6 @@ public static class VMap { private AddressableLEDBuffer m_ledBuffer; private byte _loopErrorCounter = 0; - private ScheduledExecutorService _updateLoopExecutor; private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGray); private LEDPattern m_foregroundPattern = null; @@ -49,78 +44,61 @@ public PwmLEDs(boolean isReal) { // Setup the warning for when the loop stops m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); m_loopStoppedAlert.set(false); - - // _updateLoopExecutor = Executors.newScheduledThreadPool(1); - // // Start the pattern update loop at 200Hz with a 3ms delay to running at the same time as periodic functions - // _updateLoopExecutor.scheduleAtFixedRate(this::updateLedStrip, 3, 5, java.util.concurrent.TimeUnit.MILLISECONDS); } else { // initialize nothing } } - @Override - public void periodic() { - if (!m_isRobotReal) - return; - - /** - * TODO: Test at 20ms periodic update loop, then test with ScheduledExecutorService. - * Determine if the pattern velocity is thrown off, or is simply rendered more smoothly. - */ - updateLedStrip(); - } - - public Command setBackgroundPattern(LEDPattern backgroundPattern) { - if (!m_isRobotReal) - return runOnce(() -> {}); - - return runOnce(() -> { - m_backgroundPattern = backgroundPattern; - }); - } - - public Command setForegroundPattern(LEDPattern foregroundPattern) { - if (!m_isRobotReal) - return runOnce(() -> {}); - - return runOnce(() -> { - m_foregroundPattern = foregroundPattern; - }); - } - - public Command clearForegroundPattern() { - if (!m_isRobotReal) - return runOnce(() -> {}); - - return runOnce(() -> { - m_foregroundPattern = null; - }); - } - - private void updateLedStrip() { + public void updateLedStrip() { + // If we're not running on a real robot, or we've failed too many times, do nothing. if (!m_isRobotReal || _loopErrorCounter > 3) return; try { if (m_foregroundPattern == null) { + // If we're only using a background pattern, apply it directly m_backgroundPattern.applyTo(m_ledBuffer); } else { + // If we have a foreground pattern, overlay it on a dimmed background var dimmedBackground = m_backgroundPattern.atBrightness(m_backgroundDimAmount); m_foregroundPattern.overlayOn(dimmedBackground).applyTo(m_ledBuffer); } + // Update the LED strip with the new data m_led.setData(m_ledBuffer); } catch (Exception e) { + // If we fail to update the LEDs, report the error and increment the error counter _loopErrorCounter++; DriverStation.reportError("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage(), e.getStackTrace()); + // If we've failed too many times, stop the loop and alert the user if (_loopErrorCounter > 3) { var msg = "[LEDs:ERROR] LED update loop has failed 3 times. Stopping loop."; DriverStation.reportError(msg, false); System.out.println(msg); m_loopStoppedAlert.set(true); - _updateLoopExecutor.shutdown(); } } } + + public Command setBackgroundPattern(LEDPattern backgroundPattern) { + if (!m_isRobotReal) + return runOnce(() -> {}); + + return runOnce(() -> m_backgroundPattern = backgroundPattern); + } + + public Command setForegroundPattern(LEDPattern foregroundPattern) { + if (!m_isRobotReal) + return runOnce(() -> {}); + + return runOnce(() -> m_foregroundPattern = foregroundPattern); + } + + public Command clearForegroundPattern() { + if (!m_isRobotReal) + return runOnce(() -> {}); + + return runOnce(() -> m_foregroundPattern = null); + } } From bdc8a99dafeea31a905e587545db077d4f0cbcbe Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Sun, 3 Nov 2024 16:55:23 -0600 Subject: [PATCH 09/24] better logging for drivetrain simulation --- .../drivetrain/DrivetrainIOReal.java | 4 +- .../drivetrain/DrivetrainIOSim.java | 78 +++++++++---------- .../drivetrain/DrivetrainSubsystem.java | 12 ++- .../subsystems/drivetrain/IDrivetrainIO.java | 12 +++ .../swervemodule/SwerveModuleIOInputs.java | 14 +++- .../swervemodule/SwerveModuleIOOutputs.java | 14 +++- .../swervemodule/SwerveModuleIOReal.java | 3 - .../struct/SwerveModuleIOInputsStruct.java | 50 ++++++++++++ .../struct/SwerveModuleIOOutputsStruct.java | 47 +++++++++++ .../prime/physics/LimelightPoseStruct.java | 7 +- .../prime/vision/LimelightInputsStruct.java | 7 +- 11 files changed, 193 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java index 5279c84..9e67088 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java @@ -221,7 +221,7 @@ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { /** * Gets the instantaneous states for each swerve module in FL, FR, RL, RR order */ - private SwerveModuleState[] getModuleStates() { + public SwerveModuleState[] getModuleStates() { return new SwerveModuleState[] { m_frontLeftInputs.ModuleState, m_frontRightInputs.ModuleState, @@ -233,7 +233,7 @@ private SwerveModuleState[] getModuleStates() { /** * Gets the cumulative positions for each swerve module in FL, FR, RL, RR order */ - private SwerveModulePosition[] getModulePositions() { + public SwerveModulePosition[] getModulePositions() { return new SwerveModulePosition[] { m_frontLeftInputs.ModulePosition, m_frontRightInputs.ModulePosition, diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index 8bf3359..daab3a7 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drivetrain; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -19,6 +20,7 @@ import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOSim; +@Logged(strategy = Strategy.OPT_IN) public class DrivetrainIOSim implements IDrivetrainIO { private DrivetrainIOInputs m_inputs; @@ -29,23 +31,21 @@ public class DrivetrainIOSim implements IDrivetrainIO { private SwerveDriveKinematics m_kinematics; private SwerveDrivePoseEstimator m_poseEstimator; - @Logged(name = "FrontLeftInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_frontLeftInputs = new SwerveModuleIOInputs(); - @Logged(name = "FrontRightInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_frontRightInputs = new SwerveModuleIOInputs(); - @Logged(name = "RearLeftInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_rearLeftInputs = new SwerveModuleIOInputs(); - @Logged(name = "RearRightInputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOInputs m_rearRightInputs = new SwerveModuleIOInputs(); - - @Logged(name = "FrontLeftOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_frontLeftOutputs = new SwerveModuleIOOutputs(); - @Logged(name = "FrontRightOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_frontRightOutputs = new SwerveModuleIOOutputs(); - @Logged(name = "RearLeftOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_rearLeftOutputs = new SwerveModuleIOOutputs(); - @Logged(name = "RearRightOutputs", importance = Logged.Importance.CRITICAL) - private SwerveModuleIOOutputs m_rearRightOutputs = new SwerveModuleIOOutputs(); + @Logged(name = "ModuleInputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOInputs[] m_moduleInputs = new SwerveModuleIOInputs[] { + new SwerveModuleIOInputs(), + new SwerveModuleIOInputs(), + new SwerveModuleIOInputs(), + new SwerveModuleIOInputs() + }; + + @Logged(name = "ModuleOutputs", importance = Logged.Importance.CRITICAL) + private SwerveModuleIOOutputs[] m_moduleOutputs = new SwerveModuleIOOutputs[] { + new SwerveModuleIOOutputs(), + new SwerveModuleIOOutputs(), + new SwerveModuleIOOutputs(), + new SwerveModuleIOOutputs() + }; public DrivetrainIOSim() { m_inputs = new DrivetrainIOInputs(); @@ -75,10 +75,10 @@ public DrivetrainIOSim() { @Override public DrivetrainIOInputs getInputs() { - m_frontLeftInputs = m_frontLeftModule.getInputs(); - m_frontRightInputs = m_frontRightModule.getInputs(); - m_rearLeftInputs = m_rearLeftModule.getInputs(); - m_rearRightInputs = m_rearRightModule.getInputs(); + m_moduleInputs[0] = m_frontLeftModule.getInputs(); + m_moduleInputs[1] = m_frontRightModule.getInputs(); + m_moduleInputs[2] = m_rearLeftModule.getInputs(); + m_moduleInputs[3] = m_rearRightModule.getInputs(); m_inputs.GyroAngle = Rotation2d.fromDegrees(m_gyroSim.getAngle()); // m_inputs.GyroAccelX = m_gyro.getAccelerationX().getValueAsDouble(); @@ -109,10 +109,10 @@ public void setOutputs(DrivetrainIOOutputs outputs) { break; } - m_frontLeftModule.setOutputs(m_frontLeftOutputs); - m_frontRightModule.setOutputs(m_frontRightOutputs); - m_rearLeftModule.setOutputs(m_rearLeftOutputs); - m_rearRightModule.setOutputs(m_rearRightOutputs); + m_frontLeftModule.setOutputs(m_moduleOutputs[0]); + m_frontRightModule.setOutputs(m_moduleOutputs[1]); + m_rearLeftModule.setOutputs(m_moduleOutputs[2]); + m_rearRightModule.setOutputs(m_moduleOutputs[3]); } @Override @@ -187,33 +187,33 @@ private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { * @param desiredStates */ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { - m_frontLeftOutputs.DesiredState = desiredStates[0]; - m_frontRightOutputs.DesiredState = desiredStates[1]; - m_rearLeftOutputs.DesiredState = desiredStates[2]; - m_rearRightOutputs.DesiredState = desiredStates[3]; + m_moduleOutputs[0].DesiredState = desiredStates[0]; + m_moduleOutputs[1].DesiredState = desiredStates[1]; + m_moduleOutputs[2].DesiredState = desiredStates[2]; + m_moduleOutputs[3].DesiredState = desiredStates[3]; } /** * Gets the instantaneous states for each swerve module in FL, FR, RL, RR order */ - private SwerveModuleState[] getModuleStates() { + public SwerveModuleState[] getModuleStates() { return new SwerveModuleState[] { - m_frontLeftInputs.ModuleState, - m_frontRightInputs.ModuleState, - m_rearLeftInputs.ModuleState, - m_rearRightInputs.ModuleState, + m_moduleInputs[0].ModuleState, + m_moduleInputs[1].ModuleState, + m_moduleInputs[2].ModuleState, + m_moduleInputs[3].ModuleState, }; } /** * Gets the cumulative positions for each swerve module in FL, FR, RL, RR order */ - private SwerveModulePosition[] getModulePositions() { + public SwerveModulePosition[] getModulePositions() { return new SwerveModulePosition[] { - m_frontLeftInputs.ModulePosition, - m_frontRightInputs.ModulePosition, - m_rearLeftInputs.ModulePosition, - m_rearRightInputs.ModulePosition, + m_moduleInputs[0].ModulePosition, + m_moduleInputs[1].ModulePosition, + m_moduleInputs[2].ModulePosition, + m_moduleInputs[3].ModulePosition, }; } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 280ec79..7869894 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -56,11 +56,12 @@ public class DrivetrainSubsystem extends SubsystemBase { .getEntry(); // IO - private IDrivetrainIO m_driveio; - - @Logged(name = "DriveInputs", importance = Logged.Importance.CRITICAL) + @Logged(name = "DriveIO", importance = Logged.Importance.CRITICAL) + private IDrivetrainIO m_driveio = Robot.isReal() + ? new DrivetrainIOReal() + : new DrivetrainIOSim(); private DrivetrainIOInputs m_inputs; - @Logged(name = "DriveOutputs", importance = Logged.Importance.CRITICAL) + @Logged(name = "DriveIOOutputs", importance = Logged.Importance.CRITICAL) private DrivetrainIOOutputs m_outputs; // Vision, Kinematics, odometry @@ -90,9 +91,6 @@ public DrivetrainSubsystem( m_setForegroundPatternFunc = setForegroundPatternFunc; // Create IO - m_driveio = isReal - ? new DrivetrainIOReal() - : new DrivetrainIOSim(); m_inputs = m_driveio.getInputs(); m_outputs = new DrivetrainIOOutputs(); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java b/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java index b39352c..5f18ea4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java @@ -1,12 +1,18 @@ package frc.robot.subsystems.drivetrain; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +@Logged(strategy = Strategy.OPT_IN) public interface IDrivetrainIO { + @Logged(name = "Inputs", importance = Logged.Importance.CRITICAL) public DrivetrainIOInputs getInputs(); public void setOutputs(DrivetrainIOOutputs outputs); @@ -18,4 +24,10 @@ public interface IDrivetrainIO { public void addPoseEstimatorVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDeviations); public void stopAllMotors(); + + @Logged(name = "ModuleStates", importance = Logged.Importance.CRITICAL) + public SwerveModuleState[] getModuleStates(); + + @Logged(name = "ModulePositions", importance = Logged.Importance.CRITICAL) + public SwerveModulePosition[] getModulePositions(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java index 951d17d..6f39e66 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java @@ -3,9 +3,21 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.subsystems.drivetrain.swervemodule.struct.SwerveModuleIOInputsStruct; @Logged -public class SwerveModuleIOInputs { +public class SwerveModuleIOInputs implements StructSerializable { + /** SwerveModuleIOInputs struct for serialization. */ + public static final SwerveModuleIOInputsStruct struct = new SwerveModuleIOInputsStruct(); + + public SwerveModuleIOInputs() { } + + public SwerveModuleIOInputs(SwerveModulePosition modulePosition, SwerveModuleState moduleState) { + ModulePosition = modulePosition; + ModuleState = moduleState; + } + public SwerveModulePosition ModulePosition = new SwerveModulePosition(); public SwerveModuleState ModuleState = new SwerveModuleState(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java index cf7209f..b7912eb 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java @@ -2,8 +2,20 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.subsystems.drivetrain.swervemodule.struct.SwerveModuleIOOutputsStruct; @Logged -public class SwerveModuleIOOutputs { +public class SwerveModuleIOOutputs implements StructSerializable { + + /** Struct for serialization. */ + public static final SwerveModuleIOOutputsStruct struct = new SwerveModuleIOOutputsStruct(); + + public SwerveModuleIOOutputs() { } + + public SwerveModuleIOOutputs(SwerveModuleState desiredState) { + DesiredState = desiredState; + } + public SwerveModuleState DesiredState = new SwerveModuleState(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java index f49d0f5..2737130 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java @@ -13,8 +13,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,7 +21,6 @@ import prime.control.PrimePIDConstants; import prime.util.CTREConverter; -@Logged(strategy = Strategy.OPT_IN) public class SwerveModuleIOReal implements ISwerveModuleIO { private SwerveModuleConfig m_map; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java new file mode 100644 index 0000000..38e5c4f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems.drivetrain.swervemodule.struct; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOInputs; + +public class SwerveModuleIOInputsStruct implements Struct { + + @Override + public Class getTypeClass() { + return SwerveModuleIOInputs.class; + } + + @Override + public String getTypeName() { + return SwerveModuleIOInputs.class.getName(); + } + + @Override + public int getSize() { + return SwerveModulePosition.struct.getSize() + SwerveModuleState.struct.getSize(); + } + + @Override + public String getSchema() { + return "SwerveModulePosition ModulePosition;SwerveModuleState ModuleState"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {SwerveModulePosition.struct, SwerveModuleState.struct}; + } + + @Override + public SwerveModuleIOInputs unpack(ByteBuffer bb) { + var position = SwerveModulePosition.struct.unpack(bb); + var state = SwerveModuleState.struct.unpack(bb); + return new SwerveModuleIOInputs(position, state); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleIOInputs value) { + SwerveModulePosition.struct.pack(bb, value.ModulePosition); + SwerveModuleState.struct.pack(bb, value.ModuleState); + } + +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java new file mode 100644 index 0000000..3c35041 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.drivetrain.swervemodule.struct; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; + +public class SwerveModuleIOOutputsStruct implements Struct { + + @Override + public Class getTypeClass() { + return SwerveModuleIOOutputs.class; + } + + @Override + public String getTypeName() { + return SwerveModuleIOOutputs.class.getName(); + } + + @Override + public int getSize() { + return SwerveModuleState.struct.getSize(); + } + + @Override + public String getSchema() { + return "SwerveModuleState DesiredState"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {SwerveModuleState.struct}; + } + + @Override + public SwerveModuleIOOutputs unpack(ByteBuffer bb) { + var state = SwerveModuleState.struct.unpack(bb); + return new SwerveModuleIOOutputs(state); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleIOOutputs value) { + SwerveModuleState.struct.pack(bb, value.DesiredState); + } + +} diff --git a/src/main/java/prime/physics/LimelightPoseStruct.java b/src/main/java/prime/physics/LimelightPoseStruct.java index 96094fa..13e3f68 100644 --- a/src/main/java/prime/physics/LimelightPoseStruct.java +++ b/src/main/java/prime/physics/LimelightPoseStruct.java @@ -27,7 +27,12 @@ public int getSize() { @Override public String getSchema() { - return "limelight pose"; + return "Pose3d Pose; double Timestamp; double TagCount; double TagSpan; double AvgTagDistanceMeters; double AvgTagArea; Matrix StdDeviations;"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose3d.struct, Matrix.getStruct(Nat.N3(), Nat.N1())}; } @Override diff --git a/src/main/java/prime/vision/LimelightInputsStruct.java b/src/main/java/prime/vision/LimelightInputsStruct.java index a7a285a..ba21222 100644 --- a/src/main/java/prime/vision/LimelightInputsStruct.java +++ b/src/main/java/prime/vision/LimelightInputsStruct.java @@ -32,7 +32,12 @@ public int getSize() { @Override public String getSchema() { - return "limelight inputs"; + return "Rotation2d TargetHorizontalOffset; Rotation2d TargetVerticalOffset; double TargetArea; int64 PipelineLatencyMs; int64 CapturePipelineLatencyMs; int64 TotalLatencyMs; int32 ApriltagId; double TagCount; LimelightPose FieldSpaceRobotPose; LimelightPose RedAllianceOriginFieldSpaceRobotPose; LimelightPose BlueAllianceOriginFieldSpaceRobotPose; LimelightPose TargetSpaceRobotPose; LimelightPose TargetSpaceCameraPose; LimelightPose RobotSpaceCameraPose; LimelightPose CameraSpaceTargetPose; LimelightPose RobotSpaceTargetPose;"; + } + + @Override + public Struct[] getNested() { + return new Struct[] { Rotation2d.struct, LimelightPose.struct }; } @Override From f9952e94e5d448b5d0d821a598e9c23221d63782 Mon Sep 17 00:00:00 2001 From: Mason H Date: Sun, 3 Nov 2024 18:26:19 -0600 Subject: [PATCH 10/24] fixed issue with LimelightInputsStruct which was causing field data overflows. origin was NT table casting --- src/main/java/frc/robot/Container.java | 6 +-- src/main/java/frc/robot/DriverDashboard.java | 13 +++--- .../drivetrain => maps}/DriveMap.java | 11 +++-- .../SwerveModuleMap.java} | 6 +-- .../java/frc/robot/subsystems/PwmLEDs.java | 39 ++++++----------- .../drivetrain/DrivetrainControlMode.java | 7 ---- .../drivetrain/DrivetrainIOOutputs.java | 1 + .../drivetrain/DrivetrainIOReal.java | 1 + .../drivetrain/DrivetrainIOSim.java | 1 + .../drivetrain/DrivetrainSubsystem.java | 9 +++- .../swervemodule/SwerveModuleIOReal.java | 6 +-- .../swervemodule/SwerveModuleIOSim.java | 4 +- .../robot/subsystems/vision/LimeLightNT.java | 19 ++++----- .../subsystems/vision/VisionSubsystem.java | 8 ++-- .../java/prime/physics/LimelightPose.java | 30 +++++++++++-- .../prime/physics/LimelightPoseStruct.java | 21 ++++------ .../java/prime/vision/LimelightInputs.java | 22 +++++----- .../prime/vision/LimelightInputsStruct.java | 42 ++++++++++++------- 18 files changed, 131 insertions(+), 115 deletions(-) rename src/main/java/frc/robot/{subsystems/drivetrain => maps}/DriveMap.java (86%) rename src/main/java/frc/robot/{subsystems/drivetrain/swervemodule/SwerveModuleConfig.java => maps/SwerveModuleMap.java} (92%) delete mode 100644 src/main/java/frc/robot/subsystems/drivetrain/DrivetrainControlMode.java diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index d39a4f0..e0d9300 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.*; -import frc.robot.subsystems.drivetrain.DriveMap; +import frc.robot.maps.DriveMap; import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -47,12 +47,12 @@ public class Container { public Container(boolean isReal) { try { - DriverDashboard.init(); + DriverDashboard.init(isReal); m_driverController = new PrimeXboxController(Controls.DRIVER_PORT); m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); // Create new subsystems - LEDs = new PwmLEDs(isReal); + LEDs = new PwmLEDs(); Vision = new VisionSubsystem(new String[] { DriveMap.LimelightFrontName, DriveMap.LimelightRearName diff --git a/src/main/java/frc/robot/DriverDashboard.java b/src/main/java/frc/robot/DriverDashboard.java index 9c0dcad..3d92606 100644 --- a/src/main/java/frc/robot/DriverDashboard.java +++ b/src/main/java/frc/robot/DriverDashboard.java @@ -80,7 +80,7 @@ public class DriverDashboard { * Constructs a new DriverDashboard and adds complex widgets that must be created in the constructor * @param config */ - public static void init() { + public static void init(boolean isReal) { // DriverTab // .addCamera( // "Rear Limelight", @@ -91,11 +91,12 @@ public static void init() { // .withSize(6, 6) // .withWidget(BuiltInWidgets.kCameraStream) // .withProperties(Map.of("Show controls", false, "Show crosshair", false)); - m_frontColorCam = CameraServer.startAutomaticCapture(); - m_frontColorCam.setResolution(320, 240); - m_frontColorCam.setFPS(20); - m_frontColorCam.setPixelFormat(PixelFormat.kMJPEG); - + if (isReal) { + m_frontColorCam = CameraServer.startAutomaticCapture(); + m_frontColorCam.setResolution(320, 240); + m_frontColorCam.setFPS(20); + m_frontColorCam.setPixelFormat(PixelFormat.kMJPEG); + } // DriverTab // .add(m_frontColorCam) // .withPosition(6, 0) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java b/src/main/java/frc/robot/maps/DriveMap.java similarity index 86% rename from src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java rename to src/main/java/frc/robot/maps/DriveMap.java index d69afdb..8265957 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DriveMap.java +++ b/src/main/java/frc/robot/maps/DriveMap.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.drivetrain; +package frc.robot.maps; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; @@ -6,7 +6,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.MomentOfInertia; -import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleConfig; import prime.control.PrimePIDConstants; public class DriveMap { @@ -48,7 +47,7 @@ public class DriveMap { WheelBaseMeters ); - public static final SwerveModuleConfig FrontLeftSwerveModule = new SwerveModuleConfig( + public static final SwerveModuleMap FrontLeftSwerveModule = new SwerveModuleMap( 2, 3, 4, @@ -57,7 +56,7 @@ public class DriveMap { true, new Translation2d(TrackWidthMeters / 2, WheelBaseMeters / 2) ); - public static final SwerveModuleConfig FrontRightSwerveModule = new SwerveModuleConfig( + public static final SwerveModuleMap FrontRightSwerveModule = new SwerveModuleMap( 5, 6, 7, @@ -66,7 +65,7 @@ public class DriveMap { true, new Translation2d(TrackWidthMeters / 2, -(WheelBaseMeters / 2)) ); - public static final SwerveModuleConfig RearRightSwerveModule = new SwerveModuleConfig( + public static final SwerveModuleMap RearRightSwerveModule = new SwerveModuleMap( 8, 9, 10, @@ -75,7 +74,7 @@ public class DriveMap { true, new Translation2d(-(TrackWidthMeters / 2), -(WheelBaseMeters / 2)) ); - public static final SwerveModuleConfig RearLeftSwerveModule = new SwerveModuleConfig( + public static final SwerveModuleMap RearLeftSwerveModule = new SwerveModuleMap( 11, 12, 13, diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleConfig.java b/src/main/java/frc/robot/maps/SwerveModuleMap.java similarity index 92% rename from src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleConfig.java rename to src/main/java/frc/robot/maps/SwerveModuleMap.java index 0c2605d..1788833 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleConfig.java +++ b/src/main/java/frc/robot/maps/SwerveModuleMap.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.drivetrain.swervemodule; +package frc.robot.maps; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -6,7 +6,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Current; -public class SwerveModuleConfig { +public class SwerveModuleMap { public int DriveMotorCanId; public int SteeringMotorCanId; @@ -24,7 +24,7 @@ public class SwerveModuleConfig { .withSupplyCurrentLimitEnable(true) .withSupplyCurrentLimit(Current.ofBaseUnits(40, Units.Amp)); - public SwerveModuleConfig( + public SwerveModuleMap( int driveMotorCanId, int steeringMotorCanId, int canCoderCanId, diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index c3782a0..1c4539e 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -18,7 +18,6 @@ public static class VMap { public static final double BackgroundDimAmount = 0.5; } - private boolean m_isRobotReal; private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; private byte _loopErrorCounter = 0; @@ -29,29 +28,24 @@ public static class VMap { private Alert m_loopStoppedAlert; private Dimensionless m_backgroundDimAmount = Units.Percent.of(50); - public PwmLEDs(boolean isReal) { - m_isRobotReal = isReal; - if (m_isRobotReal) { - // Initialize the LED strip and buffer - m_ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); - m_led = new AddressableLED(VMap.PwmPort); - m_led.setLength(m_ledBuffer.getLength()); - m_led.start(); + public PwmLEDs() { + // Initialize the LED strip and buffer + m_ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); + m_led = new AddressableLED(VMap.PwmPort); + m_led.setLength(m_ledBuffer.getLength()); + m_led.start(); - // Apply a default pattern to the LED strip - m_backgroundPattern.applyTo(m_ledBuffer); + // Apply a default pattern to the LED strip + m_backgroundPattern.applyTo(m_ledBuffer); - // Setup the warning for when the loop stops - m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); - m_loopStoppedAlert.set(false); - } else { - // initialize nothing - } + // Setup the warning for when the loop stops + m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); + m_loopStoppedAlert.set(false); } public void updateLedStrip() { // If we're not running on a real robot, or we've failed too many times, do nothing. - if (!m_isRobotReal || _loopErrorCounter > 3) + if (_loopErrorCounter > 3) return; try { @@ -82,23 +76,14 @@ public void updateLedStrip() { } public Command setBackgroundPattern(LEDPattern backgroundPattern) { - if (!m_isRobotReal) - return runOnce(() -> {}); - return runOnce(() -> m_backgroundPattern = backgroundPattern); } public Command setForegroundPattern(LEDPattern foregroundPattern) { - if (!m_isRobotReal) - return runOnce(() -> {}); - return runOnce(() -> m_foregroundPattern = foregroundPattern); } public Command clearForegroundPattern() { - if (!m_isRobotReal) - return runOnce(() -> {}); - return runOnce(() -> m_foregroundPattern = null); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainControlMode.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainControlMode.java deleted file mode 100644 index b3a4ce9..0000000 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainControlMode.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems.drivetrain; - -public enum DrivetrainControlMode { - kRobotRelative, - kFieldRelative, - kPathFollowing, - } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java index 2395a7e..218ff01 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java @@ -3,6 +3,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.subsystems.drivetrain.DrivetrainSubsystem.DrivetrainControlMode; @Logged public class DrivetrainIOOutputs { diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java index 9e67088..9039d88 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.Robot; +import frc.robot.maps.DriveMap; import frc.robot.subsystems.drivetrain.swervemodule.*; @Logged(strategy = Strategy.OPT_IN) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index daab3a7..a6d02af 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; import frc.robot.Robot; +import frc.robot.maps.DriveMap; import frc.robot.subsystems.drivetrain.swervemodule.ISwerveModuleIO; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOInputs; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 7869894..2fbde74 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -24,6 +24,7 @@ import frc.robot.DriverDashboard; import frc.robot.Robot; +import frc.robot.maps.DriveMap; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Map; @@ -36,6 +37,12 @@ @Logged(strategy = Strategy.OPT_IN) public class DrivetrainSubsystem extends SubsystemBase { + public enum DrivetrainControlMode { + kRobotRelative, + kFieldRelative, + kPathFollowing, + } + private Runnable m_clearForegroundPatternFunc; private Consumer m_setForegroundPatternFunc; @@ -191,7 +198,7 @@ private void evaluatePoseEstimation(boolean withinTrustedVelocity, int limelight if (isValidTarget && withinTrustedVelocity) { var llPose = limelightInputs.BlueAllianceOriginFieldSpaceRobotPose; - m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.StdDeviations); + m_driveio.addPoseEstimatorVisionMeasurement(llPose.Pose.toPose2d(), llPose.Timestamp, llPose.getStdDeviations()); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java index 2737130..bd4bd30 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java @@ -17,13 +17,13 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.subsystems.drivetrain.DriveMap; +import frc.robot.maps.*; import prime.control.PrimePIDConstants; import prime.util.CTREConverter; public class SwerveModuleIOReal implements ISwerveModuleIO { - private SwerveModuleConfig m_map; + private SwerveModuleMap m_map; private SwerveModuleIOInputs m_inputs; // Devices @@ -36,7 +36,7 @@ public class SwerveModuleIOReal implements ISwerveModuleIO { // Starts at velocity 0, no feed forward. Uses PID slot 0. private final VelocityVoltage m_voltageVelocity = new VelocityVoltage(0, 0, false, 0, 0, false, false, false, false); - public SwerveModuleIOReal(SwerveModuleConfig moduleMap) { + public SwerveModuleIOReal(SwerveModuleMap moduleMap) { m_map = moduleMap; setupSteeringMotor(DriveMap.SteeringPID); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index c132a90..25aa8b5 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -1,10 +1,12 @@ package frc.robot.subsystems.drivetrain.swervemodule; +import frc.robot.maps.SwerveModuleMap; + public class SwerveModuleIOSim implements ISwerveModuleIO { private SwerveModuleIOInputs m_inputs = new SwerveModuleIOInputs(); - public SwerveModuleIOSim(SwerveModuleConfig moduleMap) { + public SwerveModuleIOSim(SwerveModuleMap moduleMap) { } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java index 4c5d8a9..ab9af71 100644 --- a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java +++ b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java @@ -3,12 +3,9 @@ import java.util.concurrent.ExecutorService; import java.util.concurrent.Executors; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; 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.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -69,22 +66,22 @@ public double getTargetArea() { /** * The pipeline's latency contribution (ms). Add to "cl" to get total latency. */ - public long getPipelineLatencyMs() { - return (long) m_limelightTable.getEntry("tl").getDouble(0.0); + public int getPipelineLatencyMs() { + return m_limelightTable.getEntry("tl").getNumber(0).intValue(); } /** * Capture pipeline latency (ms). Time between the end of the exposure of the middle row of the sensor to the beginning of the tracking pipeline. */ - public long getCapturePipelineLatencyMs() { - return (long) m_limelightTable.getEntry("cl").getDouble(0.0); + public int getCapturePipelineLatencyMs() { + return m_limelightTable.getEntry("cl").getNumber(0).intValue(); } /** * The total latency of the capture and pipeline processing in milliseconds. * @return */ - public long getTotalLatencyMs() { + public int getTotalLatencyMs() { return getPipelineLatencyMs() + getCapturePipelineLatencyMs(); } @@ -96,7 +93,7 @@ public long getTotalLatencyMs() { * ID of the primary in-view AprilTag */ public int getApriltagId() { - return (int) m_limelightTable.getEntry("tid").getDouble(-1); + return m_limelightTable.getEntry("tid").getNumber(0).intValue(); } /** @@ -180,14 +177,14 @@ public LimelightPose getTargetPoseInRobotSpace() { * Calculates a trust value based on the number of tags in view. * @return */ - public Matrix calculateTrust(double tagCount) { + public double[] calculateTrust(double tagCount) { // Trust level is a function of the number of tags in view // var trustLevel = 0.490956d + Math.pow(9998.51d, -(6.95795d * tagCount)); var trustLevel = tagCount >= 2.0 ? 2 : 20; // X Y Z trust levels (never trust Z) - return VecBuilder.fill(trustLevel, trustLevel, 999999); + return VecBuilder.fill(trustLevel, trustLevel, 999999).getData(); } //#endregion diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 7b4c31e..40c8c36 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -13,18 +13,19 @@ public class VisionSubsystem extends SubsystemBase { private LimeLightNT[] m_limelights; - @Logged(importance = Logged.Importance.CRITICAL) private LimelightInputs[] m_limelightInputs; public VisionSubsystem(String[] tableNames) { setName("VisionSubsystem"); + SmartDashboard.putNumber("LimelightCount", tableNames.length); var defaultInstance = NetworkTableInstance.getDefault(); + m_limelights = new LimeLightNT[tableNames.length]; + m_limelightInputs = new LimelightInputs[tableNames.length]; for (int i = 0; i < tableNames.length; i++) { m_limelights[i] = new LimeLightNT(defaultInstance, tableNames[i]); + m_limelightInputs[i] = m_limelights[i].getInputs(); } - - m_limelightInputs = new LimelightInputs[tableNames.length]; } /** @@ -38,6 +39,7 @@ public LimelightInputs getLimelightInputs(int llIndex) { /** * Gets all limelight inputs */ + @Logged(name = "LimelightInputs", importance = Logged.Importance.CRITICAL) public LimelightInputs[] getAllLimelightInputs() { return m_limelightInputs; } diff --git a/src/main/java/prime/physics/LimelightPose.java b/src/main/java/prime/physics/LimelightPose.java index 327b876..c4e9073 100644 --- a/src/main/java/prime/physics/LimelightPose.java +++ b/src/main/java/prime/physics/LimelightPose.java @@ -23,10 +23,11 @@ public class LimelightPose implements StructSerializable { public double TagSpan = 0.0; public double AvgTagDistanceMeters = 0.0; public double AvgTagArea = 0.0; - @NotLogged - public Matrix StdDeviations = new Matrix<>(new SimpleMatrix(3, 1)); + public double[] StdDeviations = new double[3]; + + public LimelightPose() {} - public LimelightPose(double[] data, Matrix stdDeviations) { + public LimelightPose(double[] data, double[] stdDeviations) { if (data.length < 11 || data.length > 11) { System.err.println("Bad LL 3D Pose Data!"); return; @@ -51,7 +52,7 @@ public LimelightPose(double[] data, Matrix stdDeviations) { StdDeviations = stdDeviations; } - public LimelightPose(Pose3d pose, double[] data, Matrix stdDeviations) { + public LimelightPose(Pose3d pose, double[] data, double[] stdDeviations) { if (data.length < 5 || data.length > 5) { System.err.println("Bad LL 3D Pose Data!"); return; @@ -68,6 +69,27 @@ public LimelightPose(Pose3d pose, double[] data, Matrix stdDeviations) { StdDeviations = stdDeviations; } + public LimelightPose(Pose3d pose, double[] data) { + if (data.length < 5 || data.length > 5) { + System.err.println("Bad LL 3D Pose Data!"); + return; + } + + Pose = pose; + + var latencyMs = data[0]; + Timestamp = Timer.getFPGATimestamp() - (latencyMs / 1000.0); + TagCount = data[1]; + TagSpan = data[2]; + AvgTagDistanceMeters = data[3]; + AvgTagArea = data[4]; + } + + @NotLogged + public Matrix getStdDeviations() { + return new Matrix(new SimpleMatrix(StdDeviations)); + } + /** Struct for serialization. */ public static final LimelightPoseStruct struct = new LimelightPoseStruct(); } diff --git a/src/main/java/prime/physics/LimelightPoseStruct.java b/src/main/java/prime/physics/LimelightPoseStruct.java index 13e3f68..1ad5fad 100644 --- a/src/main/java/prime/physics/LimelightPoseStruct.java +++ b/src/main/java/prime/physics/LimelightPoseStruct.java @@ -2,8 +2,6 @@ import java.nio.ByteBuffer; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.util.struct.Struct; @@ -20,19 +18,19 @@ public String getTypeName() { @Override public int getSize() { - var size = Pose3d.struct.getSize() + (kSizeDouble * 5) + Matrix.getStruct(Nat.N3(), Nat.N1()).getSize(); + var size = Pose3d.struct.getSize() + (kSizeDouble * 5) + (kSizeDouble * 3); return size; } @Override public String getSchema() { - return "Pose3d Pose; double Timestamp; double TagCount; double TagSpan; double AvgTagDistanceMeters; double AvgTagArea; Matrix StdDeviations;"; + return "Pose3d pose;double timestamp;double tagCount;double tagSpan;double avgTagDistanceMeters;double avgTagArea;double stdDeviations[3]"; } @Override public Struct[] getNested() { - return new Struct[] {Pose3d.struct, Matrix.getStruct(Nat.N3(), Nat.N1())}; + return new Struct[] {Pose3d.struct}; } @Override @@ -41,11 +39,11 @@ public LimelightPose unpack(ByteBuffer bb) { var data = new double[5]; for (int i = 0; i < 5; i++) { - data[i] = bb.getDouble(i * kSizeDouble); + data[i] = bb.getDouble(i * kSizeDouble); } - var stdDeviations = Matrix.getStruct(Nat.N3(), Nat.N1()).unpack(bb.position(kSizeDouble * 11)); - + var stdDeviations = Struct.unpackDoubleArray(bb, 3); + return new LimelightPose(pose, data, stdDeviations); } @@ -57,11 +55,6 @@ public void pack(ByteBuffer bb, LimelightPose value) { bb.putDouble(value.TagSpan); bb.putDouble(value.AvgTagDistanceMeters); bb.putDouble(value.AvgTagArea); - Matrix.getStruct(Nat.N3(), Nat.N1()).pack(bb, value.StdDeviations); - } - - @Override - public boolean isImmutable() { - return true; + Struct.packArray(bb, value.StdDeviations); } } diff --git a/src/main/java/prime/vision/LimelightInputs.java b/src/main/java/prime/vision/LimelightInputs.java index fd8d054..f9d2a72 100644 --- a/src/main/java/prime/vision/LimelightInputs.java +++ b/src/main/java/prime/vision/LimelightInputs.java @@ -28,18 +28,18 @@ public class LimelightInputs implements StructSerializable { /** * The pipeline's latency contribution (ms). Add to "cl" to get total latency. */ - public long PipelineLatencyMs = 0; + public int PipelineLatencyMs = 0; /** * Time between the end of the exposure of the middle row of the sensor to * the beginning of the tracking pipeline. */ - public long CapturePipelineLatencyMs = 0; + public int CapturePipelineLatencyMs = 0; /** * The total latency of the capture and pipeline processing in milliseconds. */ - public long TotalLatencyMs = 0; + public int TotalLatencyMs = 0; /** * ID of the primary in-view AprilTag @@ -54,42 +54,42 @@ public class LimelightInputs implements StructSerializable { /** * Robot transform in field-space. */ - public LimelightPose FieldSpaceRobotPose = null; + public LimelightPose FieldSpaceRobotPose = new LimelightPose(); /** * Robot transform in field-space (alliance driverstation WPILIB origin). */ - public LimelightPose RedAllianceOriginFieldSpaceRobotPose = null; + public LimelightPose RedAllianceOriginFieldSpaceRobotPose = new LimelightPose(); /** * Robot transform in field-space (alliance driverstation WPILIB origin). */ - public LimelightPose BlueAllianceOriginFieldSpaceRobotPose = null; + public LimelightPose BlueAllianceOriginFieldSpaceRobotPose = new LimelightPose(); /** * 3D transform of the robot in the coordinate system of the primary in-view AprilTag */ - public LimelightPose TargetSpaceRobotPose = null; + public LimelightPose TargetSpaceRobotPose = new LimelightPose(); /** * 3D transform of the camera in the coordinate system of the primary in-view AprilTag */ - public LimelightPose TargetSpaceCameraPose = null; + public LimelightPose TargetSpaceCameraPose = new LimelightPose(); /** * 3D transform of the camera in the coordinate system of the robot */ - public LimelightPose RobotSpaceCameraPose = null; + public LimelightPose RobotSpaceCameraPose = new LimelightPose(); /** * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera */ - public LimelightPose CameraSpaceTargetPose = null; + public LimelightPose CameraSpaceTargetPose = new LimelightPose(); /** * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot */ - public LimelightPose RobotSpaceTargetPose = null; + public LimelightPose RobotSpaceTargetPose = new LimelightPose(); /** Struct for serialization. */ public static final LimelightInputsStruct struct = new LimelightInputsStruct(); diff --git a/src/main/java/prime/vision/LimelightInputsStruct.java b/src/main/java/prime/vision/LimelightInputsStruct.java index ba21222..6cbdea4 100644 --- a/src/main/java/prime/vision/LimelightInputsStruct.java +++ b/src/main/java/prime/vision/LimelightInputsStruct.java @@ -22,8 +22,7 @@ public int getSize() { // Calculate the size based on the fields in LimelightInputs int size = Rotation2d.struct.getSize() * 2; // For TargetHorizontalOffset and TargetVerticalOffset size += kSizeDouble; // For TargetArea - size += kSizeInt64 * 3; // For PipelineLatencyMs, CapturePipelineLatencyMs, TotalLatencyMs - size += kSizeInt32; // For ApriltagId + size += kSizeInt32 * 4; // For PipelineLatencyMs, CapturePipelineLatencyMs, TotalLatencyMs, and ApriltagId size += kSizeDouble; // For TagCount size += LimelightPose.struct.getSize() * 8; // For all poses @@ -32,12 +31,30 @@ public int getSize() { @Override public String getSchema() { - return "Rotation2d TargetHorizontalOffset; Rotation2d TargetVerticalOffset; double TargetArea; int64 PipelineLatencyMs; int64 CapturePipelineLatencyMs; int64 TotalLatencyMs; int32 ApriltagId; double TagCount; LimelightPose FieldSpaceRobotPose; LimelightPose RedAllianceOriginFieldSpaceRobotPose; LimelightPose BlueAllianceOriginFieldSpaceRobotPose; LimelightPose TargetSpaceRobotPose; LimelightPose TargetSpaceCameraPose; LimelightPose RobotSpaceCameraPose; LimelightPose CameraSpaceTargetPose; LimelightPose RobotSpaceTargetPose;"; + return "Rotation2d targetHorizontalOffset;"+ + "Rotation2d targetVerticalOffset;"+ + "double targetArea;"+ + "int64 pipelineLatencyMs;"+ + "int64 capturePipelineLatencyMs;"+ + "int64 totalLatencyMs;"+ + "int32 apriltagId;"+ + "double tagCount;"+ + "LimelightPose fieldSpaceRobotPose;"+ + "LimelightPose redAllianceOriginFieldSpaceRobotPose;"+ + "LimelightPose blueAllianceOriginFieldSpaceRobotPose;"+ + "LimelightPose targetSpaceRobotPose;"+ + "LimelightPose targetSpaceCameraPose;"+ + "LimelightPose robotSpaceCameraPose;"+ + "LimelightPose cameraSpaceTargetPose;"+ + "LimelightPose robotSpaceTargetPose"; } @Override public Struct[] getNested() { - return new Struct[] { Rotation2d.struct, LimelightPose.struct }; + return new Struct[] { + Rotation2d.struct, + LimelightPose.struct, + }; } @Override @@ -47,9 +64,9 @@ public LimelightInputs unpack(ByteBuffer bb) { inputs.TargetHorizontalOffset = Rotation2d.struct.unpack(bb); inputs.TargetVerticalOffset = Rotation2d.struct.unpack(bb); inputs.TargetArea = bb.getDouble(); - inputs.PipelineLatencyMs = bb.getLong(); - inputs.CapturePipelineLatencyMs = bb.getLong(); - inputs.TotalLatencyMs = bb.getLong(); + inputs.PipelineLatencyMs = bb.getInt(); + inputs.CapturePipelineLatencyMs = bb.getInt(); + inputs.TotalLatencyMs = bb.getInt(); inputs.ApriltagId = bb.getInt(); inputs.TagCount = bb.getDouble(); inputs.FieldSpaceRobotPose = LimelightPose.struct.unpack(bb); @@ -69,9 +86,9 @@ public void pack(ByteBuffer bb, LimelightInputs value) { Rotation2d.struct.pack(bb, value.TargetHorizontalOffset); Rotation2d.struct.pack(bb, value.TargetVerticalOffset); bb.putDouble(value.TargetArea); - bb.putLong(value.PipelineLatencyMs); - bb.putLong(value.CapturePipelineLatencyMs); - bb.putLong(value.TotalLatencyMs); + bb.putInt(value.PipelineLatencyMs); + bb.putInt(value.CapturePipelineLatencyMs); + bb.putInt(value.TotalLatencyMs); bb.putInt(value.ApriltagId); bb.putDouble(value.TagCount); LimelightPose.struct.pack(bb, value.FieldSpaceRobotPose); @@ -83,9 +100,4 @@ public void pack(ByteBuffer bb, LimelightInputs value) { LimelightPose.struct.pack(bb, value.CameraSpaceTargetPose); LimelightPose.struct.pack(bb, value.RobotSpaceTargetPose); } - - @Override - public boolean isImmutable() { - return true; - } } From 2247dc8a51779206cd9b5d89b7e13bbad600ce4d Mon Sep 17 00:00:00 2001 From: Mason H Date: Sun, 3 Nov 2024 23:05:58 -0600 Subject: [PATCH 11/24] trying to resolve issue with LimelightInputs showing 11 items in array when there's only two --- src/main/java/frc/robot/Container.java | 5 +---- .../java/frc/robot/subsystems/PwmLEDs.java | 2 +- .../drivetrain/DrivetrainSubsystem.java | 4 ++-- .../subsystems/vision/VisionSubsystem.java | 18 ++++++++++-------- 4 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index e0d9300..e1ff965 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -53,10 +53,7 @@ public Container(boolean isReal) { // Create new subsystems LEDs = new PwmLEDs(); - Vision = new VisionSubsystem(new String[] { - DriveMap.LimelightFrontName, - DriveMap.LimelightRearName - }); + Vision = new VisionSubsystem(); Drivetrain = new DrivetrainSubsystem(isReal, LEDs::clearForegroundPattern, LEDs::setForegroundPattern, diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index 1c4539e..6c56015 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -22,7 +22,7 @@ public static class VMap { private AddressableLEDBuffer m_ledBuffer; private byte _loopErrorCounter = 0; - private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGray); + private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kOrange); private LEDPattern m_foregroundPattern = null; private Alert m_loopStoppedAlert; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 2fbde74..1076d75 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -221,8 +221,8 @@ public void periodic() { EstimatePoseUsingFrontCamera = DriverDashboard.FrontPoseEstimationSwitch.getBoolean(false); if (EstimatePoseUsingFrontCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 0); - EstimatePoseUsingRearCamera = DriverDashboard.RearPoseEstimationSwitch.getBoolean(false); - if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); + // EstimatePoseUsingRearCamera = DriverDashboard.RearPoseEstimationSwitch.getBoolean(false); + // if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); // Update LEDs if (m_inputs.SnapIsOnTarget) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 40c8c36..09d0762 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.DriverDashboard; +import frc.robot.maps.DriveMap; import prime.vision.LimelightInputs; @Logged(strategy = Strategy.OPT_IN) @@ -15,17 +16,18 @@ public class VisionSubsystem extends SubsystemBase { private LimelightInputs[] m_limelightInputs; - public VisionSubsystem(String[] tableNames) { + public VisionSubsystem() { setName("VisionSubsystem"); - SmartDashboard.putNumber("LimelightCount", tableNames.length); var defaultInstance = NetworkTableInstance.getDefault(); - m_limelights = new LimeLightNT[tableNames.length]; - m_limelightInputs = new LimelightInputs[tableNames.length]; - for (int i = 0; i < tableNames.length; i++) { - m_limelights[i] = new LimeLightNT(defaultInstance, tableNames[i]); - m_limelightInputs[i] = m_limelights[i].getInputs(); - } + m_limelights = new LimeLightNT[] { + new LimeLightNT(defaultInstance, DriveMap.LimelightFrontName), + new LimeLightNT(defaultInstance, DriveMap.LimelightRearName) + }; + m_limelightInputs = new LimelightInputs[] { + m_limelights[0].getInputs(), + m_limelights[1].getInputs() + }; } /** From f7b95215194cb8cfc889a7a6375145e684aebaaf Mon Sep 17 00:00:00 2001 From: Mason H Date: Sun, 3 Nov 2024 23:35:15 -0600 Subject: [PATCH 12/24] final commit before submitting bug. moved some classes to consolidate folders --- .../java/frc/robot/subsystems/vision/LimeLightNT.java | 2 +- .../frc/robot/subsystems/vision/VisionSubsystem.java | 10 +++++++--- src/main/java/prime/vision/LimelightInputs.java | 1 - src/main/java/prime/vision/LimelightInputsStruct.java | 1 - .../java/prime/{physics => vision}/LimelightPose.java | 2 +- .../prime/{physics => vision}/LimelightPoseStruct.java | 2 +- 6 files changed, 10 insertions(+), 8 deletions(-) rename src/main/java/prime/{physics => vision}/LimelightPose.java (99%) rename src/main/java/prime/{physics => vision}/LimelightPoseStruct.java (98%) diff --git a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java index ab9af71..166efc2 100644 --- a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java +++ b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java @@ -11,8 +11,8 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import prime.physics.LimelightPose; import prime.vision.LimelightInputs; +import prime.vision.LimelightPose; public class LimeLightNT implements AutoCloseable { private NetworkTable m_limelightTable; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 09d0762..f263e12 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -15,6 +15,8 @@ public class VisionSubsystem extends SubsystemBase { private LimeLightNT[] m_limelights; private LimelightInputs[] m_limelightInputs; + @Logged(name = "LimelightInputCount", importance = Logged.Importance.CRITICAL) + private int limelightInputCount; public VisionSubsystem() { setName("VisionSubsystem"); @@ -28,6 +30,7 @@ public VisionSubsystem() { m_limelights[0].getInputs(), m_limelights[1].getInputs() }; + limelightInputCount = m_limelightInputs.length; } /** @@ -119,9 +122,10 @@ public void setCameraPose(int llIndex, Pose3d pose) { public void periodic() { // Update all limelight inputs - for (int i = 0; i < m_limelights.length; i++) { - m_limelightInputs[i] = m_limelights[i].getInputs(); - } + // for (int i = 0; i < m_limelights.length; i++) { + // m_limelightInputs[i] = m_limelights[i].getInputs(); + // } + limelightInputCount = m_limelightInputs.length; // Update Dashboard & logging var frontInputs = getLimelightInputs(0); diff --git a/src/main/java/prime/vision/LimelightInputs.java b/src/main/java/prime/vision/LimelightInputs.java index f9d2a72..49506da 100644 --- a/src/main/java/prime/vision/LimelightInputs.java +++ b/src/main/java/prime/vision/LimelightInputs.java @@ -3,7 +3,6 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.struct.StructSerializable; -import prime.physics.LimelightPose; @Logged public class LimelightInputs implements StructSerializable { diff --git a/src/main/java/prime/vision/LimelightInputsStruct.java b/src/main/java/prime/vision/LimelightInputsStruct.java index 6cbdea4..dc44908 100644 --- a/src/main/java/prime/vision/LimelightInputsStruct.java +++ b/src/main/java/prime/vision/LimelightInputsStruct.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.struct.Struct; -import prime.physics.LimelightPose; public class LimelightInputsStruct implements Struct { @Override diff --git a/src/main/java/prime/physics/LimelightPose.java b/src/main/java/prime/vision/LimelightPose.java similarity index 99% rename from src/main/java/prime/physics/LimelightPose.java rename to src/main/java/prime/vision/LimelightPose.java index c4e9073..e716d81 100644 --- a/src/main/java/prime/physics/LimelightPose.java +++ b/src/main/java/prime/vision/LimelightPose.java @@ -1,4 +1,4 @@ -package prime.physics; +package prime.vision; import org.ejml.simple.SimpleMatrix; diff --git a/src/main/java/prime/physics/LimelightPoseStruct.java b/src/main/java/prime/vision/LimelightPoseStruct.java similarity index 98% rename from src/main/java/prime/physics/LimelightPoseStruct.java rename to src/main/java/prime/vision/LimelightPoseStruct.java index 1ad5fad..6295dec 100644 --- a/src/main/java/prime/physics/LimelightPoseStruct.java +++ b/src/main/java/prime/vision/LimelightPoseStruct.java @@ -1,4 +1,4 @@ -package prime.physics; +package prime.vision; import java.nio.ByteBuffer; From 7211c65b36ae38f981b52955f917cdb1c262ac6d Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Mon, 4 Nov 2024 10:13:56 -0600 Subject: [PATCH 13/24] fixed scheme issue in LimelightInputsStruct. added dummy data and a limelightPose[] for testing --- .../subsystems/vision/VisionSubsystem.java | 26 ++++++++++++++----- .../java/prime/vision/LimelightInputs.java | 17 +++++++----- .../prime/vision/LimelightInputsStruct.java | 6 ++--- 3 files changed, 34 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index f263e12..cc94f3d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -9,14 +9,16 @@ import frc.robot.DriverDashboard; import frc.robot.maps.DriveMap; import prime.vision.LimelightInputs; +import prime.vision.LimelightPose; @Logged(strategy = Strategy.OPT_IN) public class VisionSubsystem extends SubsystemBase { private LimeLightNT[] m_limelights; private LimelightInputs[] m_limelightInputs; - @Logged(name = "LimelightInputCount", importance = Logged.Importance.CRITICAL) - private int limelightInputCount; + private LimelightPose[] m_limelightRobotPoses; + @Logged(name = "ArrayElementCount", importance = Logged.Importance.CRITICAL) + private int arrayElementCount; public VisionSubsystem() { setName("VisionSubsystem"); @@ -27,10 +29,14 @@ public VisionSubsystem() { new LimeLightNT(defaultInstance, DriveMap.LimelightRearName) }; m_limelightInputs = new LimelightInputs[] { - m_limelights[0].getInputs(), - m_limelights[1].getInputs() + new LimelightInputs(), + new LimelightInputs() }; - limelightInputCount = m_limelightInputs.length; + m_limelightRobotPoses = new LimelightPose[] { + m_limelightInputs[0].FieldSpaceRobotPose, + m_limelightInputs[1].FieldSpaceRobotPose + }; + arrayElementCount = m_limelightInputs.length; } /** @@ -49,6 +55,14 @@ public LimelightInputs[] getAllLimelightInputs() { return m_limelightInputs; } + /** + * Gets all limelight inputs + */ + @Logged(name = "LimelightPoses", importance = Logged.Importance.CRITICAL) + public LimelightPose[] getAllFieldRobotPoses() { + return m_limelightRobotPoses; + } + /** * Sets limelight’s LED state. * 0 = use the LED Mode set in the current pipeline. @@ -125,7 +139,7 @@ public void periodic() { // for (int i = 0; i < m_limelights.length; i++) { // m_limelightInputs[i] = m_limelights[i].getInputs(); // } - limelightInputCount = m_limelightInputs.length; + arrayElementCount = m_limelightInputs.length; // Update Dashboard & logging var frontInputs = getLimelightInputs(0); diff --git a/src/main/java/prime/vision/LimelightInputs.java b/src/main/java/prime/vision/LimelightInputs.java index 49506da..042e6bc 100644 --- a/src/main/java/prime/vision/LimelightInputs.java +++ b/src/main/java/prime/vision/LimelightInputs.java @@ -1,7 +1,10 @@ package prime.vision; import edu.wpi.first.epilogue.Logged; +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.units.Units; import edu.wpi.first.util.struct.StructSerializable; @Logged @@ -11,7 +14,7 @@ public class LimelightInputs implements StructSerializable { * Horizontal Offset From Crosshair To Target * (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) */ - public Rotation2d TargetHorizontalOffset = new Rotation2d(); + public Rotation2d TargetHorizontalOffset = new Rotation2d(Math.PI); /** * Vertical Offset From Crosshair To Target @@ -27,18 +30,18 @@ public class LimelightInputs implements StructSerializable { /** * The pipeline's latency contribution (ms). Add to "cl" to get total latency. */ - public int PipelineLatencyMs = 0; + public int PipelineLatencyMs = 1; /** * Time between the end of the exposure of the middle row of the sensor to * the beginning of the tracking pipeline. */ - public int CapturePipelineLatencyMs = 0; + public int CapturePipelineLatencyMs = 2; /** * The total latency of the capture and pipeline processing in milliseconds. */ - public int TotalLatencyMs = 0; + public int TotalLatencyMs = 3; /** * ID of the primary in-view AprilTag @@ -48,12 +51,14 @@ public class LimelightInputs implements StructSerializable { /** * Returns the number of AprilTags in the image. */ - public double TagCount = 0.0; + public double TagCount = 1.1; /** * Robot transform in field-space. */ - public LimelightPose FieldSpaceRobotPose = new LimelightPose(); + public LimelightPose FieldSpaceRobotPose = new LimelightPose( + new Pose3d(1, 2, 3, new Rotation3d(Units.Degrees.of(1), Units.Degrees.of(2), Units.Degrees.of(3))), + new double[] { 1, 1, 2.5, 3, 4 }); /** * Robot transform in field-space (alliance driverstation WPILIB origin). diff --git a/src/main/java/prime/vision/LimelightInputsStruct.java b/src/main/java/prime/vision/LimelightInputsStruct.java index dc44908..7a072ae 100644 --- a/src/main/java/prime/vision/LimelightInputsStruct.java +++ b/src/main/java/prime/vision/LimelightInputsStruct.java @@ -33,9 +33,9 @@ public String getSchema() { return "Rotation2d targetHorizontalOffset;"+ "Rotation2d targetVerticalOffset;"+ "double targetArea;"+ - "int64 pipelineLatencyMs;"+ - "int64 capturePipelineLatencyMs;"+ - "int64 totalLatencyMs;"+ + "int32 pipelineLatencyMs;"+ + "int32 capturePipelineLatencyMs;"+ + "int32 totalLatencyMs;"+ "int32 apriltagId;"+ "double tagCount;"+ "LimelightPose fieldSpaceRobotPose;"+ From 3dada05af66f5783e0dc5850e86d18766b36cb35 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Mon, 4 Nov 2024 10:17:00 -0600 Subject: [PATCH 14/24] used static unpackDoubleArray function instead of manually unpacking each data element in LimelightPoseStruct --- src/main/java/prime/vision/LimelightPoseStruct.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/prime/vision/LimelightPoseStruct.java b/src/main/java/prime/vision/LimelightPoseStruct.java index 6295dec..1866b49 100644 --- a/src/main/java/prime/vision/LimelightPoseStruct.java +++ b/src/main/java/prime/vision/LimelightPoseStruct.java @@ -36,15 +36,10 @@ public Struct[] getNested() { @Override public LimelightPose unpack(ByteBuffer bb) { var pose = Pose3d.struct.unpack(bb); - - var data = new double[5]; - for (int i = 0; i < 5; i++) { - data[i] = bb.getDouble(i * kSizeDouble); - } - + var tagData = Struct.unpackDoubleArray(bb, 5); var stdDeviations = Struct.unpackDoubleArray(bb, 3); - return new LimelightPose(pose, data, stdDeviations); + return new LimelightPose(pose, tagData, stdDeviations); } @Override From c1b0a403f258f5cacf0abc7365209f37be41cb2c Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Mon, 4 Nov 2024 10:21:47 -0600 Subject: [PATCH 15/24] better naming in LimelightPose constructors --- src/main/java/prime/vision/LimelightPose.java | 60 +++++++++++-------- 1 file changed, 35 insertions(+), 25 deletions(-) diff --git a/src/main/java/prime/vision/LimelightPose.java b/src/main/java/prime/vision/LimelightPose.java index e716d81..5ca0f7e 100644 --- a/src/main/java/prime/vision/LimelightPose.java +++ b/src/main/java/prime/vision/LimelightPose.java @@ -27,62 +27,72 @@ public class LimelightPose implements StructSerializable { public LimelightPose() {} - public LimelightPose(double[] data, double[] stdDeviations) { - if (data.length < 11 || data.length > 11) { + public LimelightPose(double[] poseTagPipelineData, double[] stdDeviations) { + if (poseTagPipelineData.length < 11 || poseTagPipelineData.length > 11) { System.err.println("Bad LL 3D Pose Data!"); return; } + if (stdDeviations.length < 3 || stdDeviations.length > 3) { + System.err.println("Bad LL StdDeviations Data!"); + return; + } + Pose = new Pose3d( - new Translation3d(data[0], data[1], data[2]), + new Translation3d(poseTagPipelineData[0], poseTagPipelineData[1], poseTagPipelineData[2]), new Rotation3d( - Units.degreesToRadians(data[3]), - Units.degreesToRadians(data[4]), - Units.degreesToRadians(data[5]) + Units.degreesToRadians(poseTagPipelineData[3]), + Units.degreesToRadians(poseTagPipelineData[4]), + Units.degreesToRadians(poseTagPipelineData[5]) ) ); - var latencyMs = data[6]; + var latencyMs = poseTagPipelineData[6]; Timestamp = Timer.getFPGATimestamp() - (latencyMs / 1000.0); - TagCount = data[7]; - TagSpan = data[8]; - AvgTagDistanceMeters = data[9]; - AvgTagArea = data[10]; + TagCount = poseTagPipelineData[7]; + TagSpan = poseTagPipelineData[8]; + AvgTagDistanceMeters = poseTagPipelineData[9]; + AvgTagArea = poseTagPipelineData[10]; StdDeviations = stdDeviations; } - public LimelightPose(Pose3d pose, double[] data, double[] stdDeviations) { - if (data.length < 5 || data.length > 5) { + public LimelightPose(Pose3d pose, double[] tagPipelineData, double[] stdDeviations) { + if (tagPipelineData.length < 5 || tagPipelineData.length > 5) { System.err.println("Bad LL 3D Pose Data!"); return; } + if (stdDeviations.length < 3 || stdDeviations.length > 3) { + System.err.println("Bad LL StdDeviations Data!"); + return; + } + Pose = pose; - var latencyMs = data[0]; + var latencyMs = tagPipelineData[0]; Timestamp = Timer.getFPGATimestamp() - (latencyMs / 1000.0); - TagCount = data[1]; - TagSpan = data[2]; - AvgTagDistanceMeters = data[3]; - AvgTagArea = data[4]; + TagCount = tagPipelineData[1]; + TagSpan = tagPipelineData[2]; + AvgTagDistanceMeters = tagPipelineData[3]; + AvgTagArea = tagPipelineData[4]; StdDeviations = stdDeviations; } - public LimelightPose(Pose3d pose, double[] data) { - if (data.length < 5 || data.length > 5) { + public LimelightPose(Pose3d pose, double[] tagPipelineData) { + if (tagPipelineData.length < 5 || tagPipelineData.length > 5) { System.err.println("Bad LL 3D Pose Data!"); return; } Pose = pose; - var latencyMs = data[0]; + var latencyMs = tagPipelineData[0]; Timestamp = Timer.getFPGATimestamp() - (latencyMs / 1000.0); - TagCount = data[1]; - TagSpan = data[2]; - AvgTagDistanceMeters = data[3]; - AvgTagArea = data[4]; + TagCount = tagPipelineData[1]; + TagSpan = tagPipelineData[2]; + AvgTagDistanceMeters = tagPipelineData[3]; + AvgTagArea = tagPipelineData[4]; } @NotLogged From bf7dc05a9463a9ad5d561ea14d76e6bdc8058e59 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Tue, 5 Nov 2024 21:05:08 -0600 Subject: [PATCH 16/24] got simulation tracking drivetrain, modules, chassis speeds, and gyro properly --- simgui-ds.json | 6 ++ src/main/java/frc/robot/Container.java | 4 +- .../drivetrain/DrivetrainIOSim.java | 11 +-- .../drivetrain/DrivetrainSubsystem.java | 61 +++++++++--- .../swervemodule/SwerveModuleIOSim.java | 92 ++++++++++++++++++- 5 files changed, 152 insertions(+), 22 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index e1ff965..c6c91d2 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -49,7 +49,7 @@ public Container(boolean isReal) { try { DriverDashboard.init(isReal); m_driverController = new PrimeXboxController(Controls.DRIVER_PORT); - m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); + // m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); // Create new subsystems LEDs = new PwmLEDs(); @@ -115,7 +115,7 @@ public void configureDriverControls() { // Controls for Driving m_driverController.a().onTrue(Drivetrain.resetGyroCommand()); Drivetrain.setDefaultCommand( - Drivetrain.defaultDriveCommand( + Drivetrain.driveRobotRelativeCommand( m_driverController.getSwerveControlProfile( HolonomicControlStyle.Drone, DriveMap.DriveDeadband, diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index a6d02af..4c918a5 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; import frc.robot.Robot; import frc.robot.maps.DriveMap; -import frc.robot.subsystems.drivetrain.swervemodule.ISwerveModuleIO; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOInputs; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOSim; @@ -27,7 +26,7 @@ public class DrivetrainIOSim implements IDrivetrainIO { private DrivetrainIOInputs m_inputs; private AnalogGyroSim m_gyroSim; - private ISwerveModuleIO m_frontLeftModule, m_frontRightModule, m_rearLeftModule, m_rearRightModule; + private SwerveModuleIOSim m_frontLeftModule, m_frontRightModule, m_rearLeftModule, m_rearRightModule; private SwerveDriveKinematics m_kinematics; private SwerveDrivePoseEstimator m_poseEstimator; @@ -81,14 +80,14 @@ public DrivetrainIOInputs getInputs() { m_moduleInputs[2] = m_rearLeftModule.getInputs(); m_moduleInputs[3] = m_rearRightModule.getInputs(); - m_inputs.GyroAngle = Rotation2d.fromDegrees(m_gyroSim.getAngle()); - // m_inputs.GyroAccelX = m_gyro.getAccelerationX().getValueAsDouble(); - // m_inputs.GyroAccelY = m_gyro.getAccelerationY().getValueAsDouble(); - // m_inputs.GyroAccelZ = m_gyro.getAccelerationZ().getValueAsDouble(); m_inputs.RobotRelativeChassisSpeeds = m_kinematics.toChassisSpeeds(getModuleStates()); + m_gyroSim.setAngle(new Rotation2d(Rotation2d.fromDegrees(m_gyroSim.getAngle()).getRadians() + m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond * 0.02).getDegrees()); + var modulePositions = getModulePositions(); m_inputs.EstimatedRobotPose = m_poseEstimator.update(m_inputs.GyroAngle, modulePositions); + m_inputs.GyroAngle = Rotation2d.fromDegrees(m_gyroSim.getAngle()); + return m_inputs; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 1076d75..8c6b975 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -163,6 +163,15 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds) { m_outputs.DesiredChassisSpeeds = desiredChassisSpeeds; } + /** + * Drives field-relative using a ChassisSpeeds + * @param desiredChassisSpeeds The desired field-relative speeds of the robot + */ + private void driveFieldRelative(ChassisSpeeds desiredChassisSpeeds) { + m_outputs.ControlMode = DrivetrainControlMode.kFieldRelative; + m_outputs.DesiredChassisSpeeds = desiredChassisSpeeds; + } + private void drivePathPlanner(ChassisSpeeds pathSpeeds) { m_outputs.ControlMode = DrivetrainControlMode.kPathFollowing; m_outputs.DesiredChassisSpeeds = pathSpeeds; @@ -247,7 +256,39 @@ public void periodic() { * Creates a command that drives the robot using input controls * @param controlSuppliers Controller input suppliers */ - public Command defaultDriveCommand(SwerveControlSuppliers controlSuppliers) { + public Command driveFieldRelativeCommand(SwerveControlSuppliers controlSuppliers) { + return this.run(() -> { + // If the driver is trying to rotate the robot, disable snap-to control + if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { + setSnapToEnabled(false); + m_clearForegroundPatternFunc.run(); + } + + // Convert inputs to MPS + var inputXMPS = controlSuppliers.X.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputYMPS = -controlSuppliers.Y.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; + + // Build chassis speeds + var invert = Robot.onRedAlliance() ? -1 : 1; + + // Drive the robot with the driver-relative inputs, converted to field-relative based on which side we're on + var fieldChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + (inputYMPS * invert), // Use Y as X for field-relative + (inputXMPS * invert), // Use X as Y for field-relative + inputRotationRadiansPS, + m_inputs.GyroAngle + ); + + driveFieldRelative(fieldChassisSpeeds); + }); + } + + /** + * Creates a command that drives the robot using input controls + * @param controlSuppliers Controller input suppliers + */ + public Command driveRobotRelativeCommand(SwerveControlSuppliers controlSuppliers) { return this.run(() -> { // If the driver is trying to rotate the robot, disable snap-to control if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { @@ -261,19 +302,17 @@ public Command defaultDriveCommand(SwerveControlSuppliers controlSuppliers) { var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; // Build chassis speeds - ChassisSpeeds robotRelativeSpeeds; var invert = Robot.onRedAlliance() ? -1 : 1; // Drive the robot with the driver-relative inputs, converted to field-relative based on which side we're on - robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - (inputYMPS * invert), // Use Y as X for field-relative - (inputXMPS * invert), // Use X as Y for field-relative - inputRotationRadiansPS, - m_inputs.GyroAngle - ); - - driveRobotRelative(robotRelativeSpeeds); + var robotChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + (inputYMPS * invert), // Use Y as X for field-relative + (inputXMPS * invert), // Use X as Y for field-relative + inputRotationRadiansPS, + m_inputs.GyroAngle + ); + + driveRobotRelative(robotChassisSpeeds); }); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index 25aa8b5..e8419a7 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -1,27 +1,113 @@ package frc.robot.subsystems.drivetrain.swervemodule; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.maps.DriveMap; import frc.robot.maps.SwerveModuleMap; +import prime.control.PrimePIDConstants; public class SwerveModuleIOSim implements ISwerveModuleIO { + private SwerveModuleMap m_Map; private SwerveModuleIOInputs m_inputs = new SwerveModuleIOInputs(); + // Devices + private DCMotorSim m_driveMotorSim; + private PIDController m_driveFeedback; + private SimpleMotorFeedforward m_driveFeedforward; + private Rotation2d m_steerAngle = new Rotation2d(); + public SwerveModuleIOSim(SwerveModuleMap moduleMap) { + m_Map = moduleMap; + + setupDriveMotor(DriveMap.DrivePID); } @Override public SwerveModuleIOInputs getInputs() { + m_driveMotorSim.update(0.020); + var speedMps = m_driveMotorSim.getAngularVelocity().in(Units.RotationsPerSecond) * DriveMap.DriveWheelCircumferenceMeters; + + m_inputs.ModuleState.angle = m_steerAngle; + m_inputs.ModuleState.speedMetersPerSecond = speedMps; + m_inputs.ModulePosition.angle = m_steerAngle; + m_inputs.ModulePosition.distanceMeters = m_driveMotorSim.getAngularPositionRotations() * DriveMap.DriveWheelCircumferenceMeters; + return m_inputs; } @Override public void setOutputs(SwerveModuleIOOutputs outputs) { - // Right now, just assume that the module is perfect and has no losses. - // This may be replaced with a more realistic simulation later. - m_inputs.ModuleState = outputs.DesiredState; + setDesiredState(outputs.DesiredState); } @Override public void stopMotors() { + m_driveMotorSim.setInputVoltage(0); + m_driveMotorSim.setAngularVelocity(0); + } + + /** + * Configures the drive motors + * @param pid + */ + private void setupDriveMotor(PrimePIDConstants pid) { + m_driveMotorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getFalcon500(1), + 0.001, + DriveMap.DriveGearRatio + ), + DCMotor.getFalcon500(1) + ); + + m_driveFeedback = new PIDController(0.1, 0, 0); + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.085); + } + + /** + * Sets the desired state of the module. + * + * @param desiredState The optimized state of the module that we'd like to be at in this + * period + */ + private void setDesiredState(SwerveModuleState desiredState) { + // Optimize the desired state + desiredState = optimize(desiredState); + + // Set the drive motor to the desired speed + // Calculate target data to voltage data + var velocityRadPerSec = desiredState.speedMetersPerSecond / (DriveMap.DriveWheelDiameterMeters / 2); + var driveAppliedVolts = m_driveFeedforward.calculate(velocityRadPerSec) + + m_driveFeedback.calculate(m_driveMotorSim.getAngularVelocityRadPerSec(), velocityRadPerSec); + driveAppliedVolts = MathUtil.clamp(driveAppliedVolts, -12.0, 12.0); + + m_driveMotorSim.setInputVoltage(driveAppliedVolts); + + m_steerAngle = desiredState.angle; + } + + /** + * Optimizes the module angle & drive inversion to ensure the module takes the shortest path to drive at the desired angle + * @param desiredState + */ + private SwerveModuleState optimize(SwerveModuleState desiredState) { + Rotation2d currentAngle = m_inputs.ModulePosition.angle; + var delta = desiredState.angle.minus(currentAngle); + if (Math.abs(delta.getDegrees()) > 90.0) { + return new SwerveModuleState( + -desiredState.speedMetersPerSecond, + desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)) + ); + } else { + return desiredState; + } } } From 17f298af886650b69631fa248fdb7a3000383fb5 Mon Sep 17 00:00:00 2001 From: zeroClearAmerican Date: Tue, 5 Nov 2024 21:37:27 -0600 Subject: [PATCH 17/24] fixed LEDs not switching patterns --- src/main/java/frc/robot/Container.java | 3 +- src/main/java/frc/robot/Robot.java | 16 +++++++- .../java/frc/robot/subsystems/PwmLEDs.java | 41 +++++++++++++++---- .../drivetrain/DrivetrainSubsystem.java | 14 ++++--- 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index c6c91d2..4c625e9 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -11,9 +11,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.epilogue.Logged.Strategy; -import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj2.command.Command; @@ -40,6 +38,7 @@ public class Container { // public Intake Intake; // public Climbers Climbers; // public PwmLEDs LEDs; + @Logged(name="LEDs", importance = Importance.CRITICAL) public PwmLEDs LEDs; // public Compressor Compressor; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9313199..f59f5c5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -65,8 +65,8 @@ public Robot() { @Override public void disabledInit() { - var disabledPattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) - .breathe(Units.Seconds.of(2.0)); + DataLogManager.log("Robot disabled"); + var disabledPattern = LEDPattern.solid(getAllianceColor()).breathe(Units.Seconds.of(2.0)); m_robotContainer.LEDs.setBackgroundPattern(disabledPattern); m_robotContainer.LEDs.clearForegroundPattern(); } @@ -125,6 +125,7 @@ public void autonomousInit() { */ @Override public void teleopInit() { + DataLogManager.log("Teleop Enabled"); if (m_autonomousCommand != null) { // Cancel the auto command if it's still running m_autonomousCommand.cancel(); @@ -163,4 +164,15 @@ public static boolean onBlueAlliance() { return alliance.isPresent() && alliance.get() == Alliance.Blue; } + + public static Color getAllianceColor() { + var alliance = DriverStation.getAlliance(); + Color allianceColor = Color.kGhostWhite; + if (alliance.isPresent()) + allianceColor = alliance.get() == Alliance.Red + ? Color.kRed + : Color.kBlue; + + return allianceColor; + } } diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index 6c56015..eb0a98d 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -1,16 +1,19 @@ package frc.robot.subsystems; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +@Logged(strategy = Logged.Strategy.OPT_IN) public class PwmLEDs extends SubsystemBase { public static class VMap { public static final int PwmPort = 9; @@ -20,9 +23,11 @@ public static class VMap { private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; - private byte _loopErrorCounter = 0; + @Logged(name = "LoopErrorCounter", importance = Logged.Importance.CRITICAL) + public byte _loopErrorCounter = 0; - private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kOrange); + private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGhostWhite) + .breathe(Units.Seconds.of(4)); private LEDPattern m_foregroundPattern = null; private Alert m_loopStoppedAlert; @@ -63,6 +68,7 @@ public void updateLedStrip() { } catch (Exception e) { // If we fail to update the LEDs, report the error and increment the error counter _loopErrorCounter++; + DataLogManager.log("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage()); DriverStation.reportError("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage(), e.getStackTrace()); // If we've failed too many times, stop the loop and alert the user @@ -75,15 +81,34 @@ public void updateLedStrip() { } } - public Command setBackgroundPattern(LEDPattern backgroundPattern) { - return runOnce(() -> m_backgroundPattern = backgroundPattern); + public void setBackgroundPattern(LEDPattern backgroundPattern) { + if (m_backgroundPattern != backgroundPattern) { + m_backgroundPattern = backgroundPattern; + } + } + + public void setForegroundPattern(LEDPattern foregroundPattern) { + if (DriverStation.isEnabled() && m_foregroundPattern != foregroundPattern) { + m_foregroundPattern = foregroundPattern; + } + } + + public void clearForegroundPattern() { + if (m_foregroundPattern != null) + m_foregroundPattern = null; + } + + public Command setBackgroundPatternCommand(LEDPattern backgroundPattern) { + return runOnce(() -> setBackgroundPattern(backgroundPattern)) + .ignoringDisable(true); } - public Command setForegroundPattern(LEDPattern foregroundPattern) { - return runOnce(() -> m_foregroundPattern = foregroundPattern); + public Command setForegroundPatternCommand(LEDPattern foregroundPattern) { + return runOnce(() -> setForegroundPattern(foregroundPattern)) + .ignoringDisable(true); // (only allowed when enabled) } - public Command clearForegroundPattern() { - return runOnce(() -> m_foregroundPattern = null); + public Command clearForegroundPatternCommand() { + return runOnce(() -> clearForegroundPattern()).ignoringDisable(true); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 8c6b975..2866873 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -29,6 +29,7 @@ import java.util.Map; import java.util.function.Consumer; +import java.util.function.Function; import java.util.function.Supplier; import prime.control.SwerveControlSuppliers; @@ -80,7 +81,8 @@ public enum DrivetrainControlMode { @Logged(importance = Logged.Importance.CRITICAL) public boolean WithinPoseEstimationVelocity = true; - private LEDPattern m_snapOnTargetPattern = LEDPattern.solid(Color.kGreen); + private LEDPattern m_snapOnTargetPattern = LEDPattern.solid(Color.kGreen) + .blink(Units.Seconds.of(0.1)); private LEDPattern m_snapOffTargetPattern = LEDPattern.steps(Map.of(0.0, Color.kRed, 0.25, Color.kBlack)) .scrollAtRelativeSpeed(Units.Hertz.of(2)); @@ -234,10 +236,12 @@ public void periodic() { // if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); // Update LEDs - if (m_inputs.SnapIsOnTarget) { - m_setForegroundPatternFunc.accept(m_snapOnTargetPattern); - } else { - m_setForegroundPatternFunc.accept(m_snapOffTargetPattern); + if (m_outputs.SnapEnabled) { + if (m_inputs.SnapIsOnTarget) { + m_setForegroundPatternFunc.accept(m_snapOnTargetPattern); + } else { + m_setForegroundPatternFunc.accept(m_snapOffTargetPattern); + } } // Send outputs to the drive IO From b3751215a20b926088d6f5170d97fdc605c86bb0 Mon Sep 17 00:00:00 2001 From: Mason H Date: Wed, 6 Nov 2024 22:32:54 -0600 Subject: [PATCH 18/24] made a cooler autonomous LED pattern --- src/main/java/frc/robot/Robot.java | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f59f5c5..c062b2a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,12 +14,14 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.PwmLEDs; @Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { @@ -87,9 +89,15 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - var autoPattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) - .blink(Units.Seconds.of(0.25)); - m_robotContainer.LEDs.setBackgroundPattern(autoPattern); + var autoPattern = LEDPattern.gradient(GradientType.kDiscontinuous, getAllianceColor(), Color.kBlack) + .offsetBy(-PwmLEDs.VMap.PixelsPerStrip / 2) + .scrollAtRelativeSpeed(Units.Hertz.of(2)) + .reversed(); + var combinedPattern = LEDPattern.gradient(GradientType.kDiscontinuous, getAllianceColor(), Color.kBlack) + .offsetBy(PwmLEDs.VMap.PixelsPerStrip / 2) + .scrollAtRelativeSpeed(Units.Hertz.of(2)) + .blend(autoPattern); + m_robotContainer.LEDs.setBackgroundPattern(combinedPattern); m_robotContainer.LEDs.clearForegroundPattern(); // Cancel any auto command that's still running and reset the subsystem states @@ -136,8 +144,7 @@ public void teleopInit() { } // Set teleop LED pattern - var telePattern = LEDPattern.solid(onRedAlliance() ? Color.kRed : Color.kBlue) - .scrollAtRelativeSpeed(Units.Hertz.of(2)); + var telePattern = LEDPattern.solid(getAllianceColor()).scrollAtRelativeSpeed(Units.Hertz.of(2)); m_robotContainer.LEDs.setBackgroundPattern(telePattern); m_robotContainer.LEDs.clearForegroundPattern(); From 866ad9c47b9271b0f8ae9ef9c4c9be2915f3313f Mon Sep 17 00:00:00 2001 From: Mason H <35619771+zeroClearAmerican@users.noreply.github.com> Date: Sat, 9 Nov 2024 00:18:16 -0600 Subject: [PATCH 19/24] linux execute permission added to gradlew --- gradlew | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 From 80577bde468f372b05c1c4dc72849c14b2305456 Mon Sep 17 00:00:00 2001 From: Mason H <35619771+zeroClearAmerican@users.noreply.github.com> Date: Mon, 18 Nov 2024 22:31:57 -0600 Subject: [PATCH 20/24] added auto-formatter and formatted drive files --- .vscode/settings.json | 6 +- formatter.xml | 338 +++++++++++ src/main/java/frc/robot/CombinedCommands.java | 76 ++- src/main/java/frc/robot/Container.java | 78 ++- src/main/java/frc/robot/DriverDashboard.java | 101 ++-- src/main/java/frc/robot/Robot.java | 25 +- .../java/frc/robot/subsystems/PwmLEDs.java | 169 +++--- .../drivetrain/DrivetrainIOInputs.java | 18 +- .../drivetrain/DrivetrainIOOutputs.java | 8 +- .../drivetrain/DrivetrainIOReal.java | 38 +- .../drivetrain/DrivetrainIOSim.java | 59 +- .../drivetrain/DrivetrainSubsystem.java | 208 +++---- .../subsystems/drivetrain/IDrivetrainIO.java | 9 +- .../swervemodule/SwerveModuleIOInputs.java | 18 +- .../swervemodule/SwerveModuleIOOutputs.java | 14 +- .../swervemodule/SwerveModuleIOReal.java | 60 +- .../swervemodule/SwerveModuleIOSim.java | 38 +- .../struct/SwerveModuleIOInputsStruct.java | 76 +-- .../struct/SwerveModuleIOOutputsStruct.java | 72 +-- .../robot/subsystems/vision/LimeLightNT.java | 550 +++++++++--------- .../subsystems/vision/VisionSubsystem.java | 291 ++++----- 21 files changed, 1255 insertions(+), 997 deletions(-) create mode 100644 formatter.xml diff --git a/.vscode/settings.json b/.vscode/settings.json index a505b56..b1d79f8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,12 +26,14 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", + "java.format.settings.url": "formatter.xml", "[java]": { "editor.formatOnSave": true, "editor.formatOnPaste": true, - "editor.defaultFormatter": "esbenp.prettier-vscode", "editor.codeActionsOnSave": [ "editor.action.formatDocument", - ] + ], + "editor.detectIndentation": false, + "editor.tabSize": 2 }, } diff --git a/formatter.xml b/formatter.xml new file mode 100644 index 0000000..4b685fc --- /dev/null +++ b/formatter.xml @@ -0,0 +1,338 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/main/java/frc/robot/CombinedCommands.java b/src/main/java/frc/robot/CombinedCommands.java index f923797..49381e5 100644 --- a/src/main/java/frc/robot/CombinedCommands.java +++ b/src/main/java/frc/robot/CombinedCommands.java @@ -11,53 +11,45 @@ import frc.robot.subsystems.Shooter; public class CombinedCommands { - /** - * Runs a sequence to score a note in the speaker - * @return - */ - public SequentialCommandGroup scoreInSpeakerSequentialGroup(Shooter shooter, Intake intake) { - return shooter - .startShootingNoteCommand() - .andThen(new WaitCommand(0.75)) - .andThen(intake.ejectNoteCommand()) - .andThen(new WaitCommand(0.75)) - .andThen(shooter.stopMotorsCommand()) - .andThen(intake.stopRollersCommand()); - } + /** + * Runs a sequence to score a note in the speaker + * + * @return + */ + public SequentialCommandGroup scoreInSpeakerSequentialGroup(Shooter shooter, Intake intake) { + return shooter.startShootingNoteCommand().andThen(new WaitCommand(0.75)).andThen(intake.ejectNoteCommand()) + .andThen(new WaitCommand(0.75)).andThen(shooter.stopMotorsCommand()).andThen(intake.stopRollersCommand()); + } - /** - * Runs a sequence to load a note into the shooter for scoring in the amp - * @return - */ - public SequentialCommandGroup loadNoteForAmp(Shooter shooter, Intake intake) { - return Commands - .runOnce(() -> intake.runIntakeRollers(-0.7)) // Eject from the intake + /** + * Runs a sequence to load a note into the shooter for scoring in the amp + * + * @return + */ + public SequentialCommandGroup loadNoteForAmp(Shooter shooter, Intake intake) { + return Commands.runOnce(() -> intake.runIntakeRollers(-0.7)) // Eject from the intake .alongWith(Commands.runOnce(() -> shooter.runShooter(0.1))) // Load into the shooter .andThen(new WaitUntilCommand(shooter::isNoteLoaded).withTimeout(1)) // Wait until the note is loaded .andThen(new WaitCommand(0.045)) // Give the note time to get into the shooter .andThen(stopShooterAndIntakeCommand(shooter, intake)); // Stop both the shooter and intake - } + } - /** - * Runs a sequence to stop both the shooter and intake - * @return - */ - public SequentialCommandGroup stopShooterAndIntakeCommand(Shooter shooter, Intake intake) { - return shooter.stopMotorsCommand().andThen(intake.stopRollersCommand()); - } + /** + * Runs a sequence to stop both the shooter and intake + * + * @return + */ + public SequentialCommandGroup stopShooterAndIntakeCommand(Shooter shooter, Intake intake) { + return shooter.stopMotorsCommand().andThen(intake.stopRollersCommand()); + } - /** - * Returns a map of named commands that use multiple subsystems - * @return - */ - public Map getNamedCommands(Shooter shooter, Intake intake) { - return Map.of( - "Score_In_Speaker", - scoreInSpeakerSequentialGroup(shooter, intake), - "Load_Note_For_Amp", - loadNoteForAmp(shooter, intake), - "Stop_Shooter_And_Intake", - stopShooterAndIntakeCommand(shooter, intake) - ); - } + /** + * Returns a map of named commands that use multiple subsystems + * + * @return + */ + public Map getNamedCommands(Shooter shooter, Intake intake) { + return Map.of("Score_In_Speaker", scoreInSpeakerSequentialGroup(shooter, intake), "Load_Note_For_Amp", + loadNoteForAmp(shooter, intake), "Stop_Shooter_And_Intake", stopShooterAndIntakeCommand(shooter, intake)); + } } diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index 4c625e9..3fee3d3 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -30,15 +30,15 @@ public class Container { private PrimeXboxController m_driverController; private PrimeXboxController m_operatorController; - @Logged(name="Vision", importance = Importance.CRITICAL) + @Logged(name = "Vision", importance = Importance.CRITICAL) public VisionSubsystem Vision; - @Logged(name="Drive", importance = Importance.CRITICAL) + @Logged(name = "Drive", importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; // public Shooter Shooter; // public Intake Intake; // public Climbers Climbers; // public PwmLEDs LEDs; - @Logged(name="LEDs", importance = Importance.CRITICAL) + @Logged(name = "LEDs", importance = Importance.CRITICAL) public PwmLEDs LEDs; // public Compressor Compressor; @@ -53,13 +53,11 @@ public Container(boolean isReal) { // Create new subsystems LEDs = new PwmLEDs(); Vision = new VisionSubsystem(); - Drivetrain = new DrivetrainSubsystem(isReal, - LEDs::clearForegroundPattern, - LEDs::setForegroundPattern, - Vision::getAllLimelightInputs); + Drivetrain = new DrivetrainSubsystem(isReal, LEDs::clearForegroundPattern, LEDs::setForegroundPattern, + Vision::getAllLimelightInputs); // Shooter = new Shooter( - // LEDs::clearForegroundPattern, - // LEDs::setForegroundPattern); + // LEDs::clearForegroundPattern, + // LEDs::setForegroundPattern); // Intake = new Intake(); // Climbers = new Climbers(); // Compressor = new Compressor(30, PneumaticsModuleType.REVPH); @@ -71,7 +69,8 @@ public Container(boolean isReal) { NamedCommands.registerCommands(Drivetrain.getNamedCommands()); // NamedCommands.registerCommands(Intake.getNamedCommands()); // NamedCommands.registerCommands(Shooter.getNamedCommands()); - // NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // Register the combined named commands that use multiple subsystems + // NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // + // Register the combined named commands that use multiple subsystems // Create Auto chooser and Auto tab in Shuffleboard configAutonomousDashboardItems(); @@ -101,6 +100,7 @@ public void configAutonomousDashboardItems() { /** * Returns the selected autonomous command to run + * * @return */ public Command getAutonomousCommand() { @@ -113,15 +113,8 @@ public Command getAutonomousCommand() { public void configureDriverControls() { // Controls for Driving m_driverController.a().onTrue(Drivetrain.resetGyroCommand()); - Drivetrain.setDefaultCommand( - Drivetrain.driveRobotRelativeCommand( - m_driverController.getSwerveControlProfile( - HolonomicControlStyle.Drone, - DriveMap.DriveDeadband, - DriveMap.DeadbandCurveWeight - ) - ) - ); + Drivetrain.setDefaultCommand(Drivetrain.driveFieldRelativeCommand(m_driverController + .getSwerveControlProfile(HolonomicControlStyle.Drone, DriveMap.DriveDeadband, DriveMap.DeadbandCurveWeight))); // While holding b, auto-aim the robot to the apriltag target using snap-to m_driverController.leftStick().whileTrue(Drivetrain.enableLockOn()).onFalse(Drivetrain.disableSnapToCommand()); @@ -137,12 +130,12 @@ public void configureDriverControls() { // m_driverController.y().onTrue(Climbers.toggleClimbControlsCommand()); // m_driverController.start().onTrue(Climbers.setArmsUpCommand()); // Climbers.setDefaultCommand( - // Climbers.defaultClimbingCommand( - // m_driverController.button(Controls.RB), - // m_driverController.button(Controls.LB), - // () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), - // () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER) - // ) + // Climbers.defaultClimbingCommand( + // m_driverController.button(Controls.RB), + // m_driverController.button(Controls.LB), + // () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), + // () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER) + // ) // ); } @@ -151,35 +144,36 @@ public void configureDriverControls() { */ public void configureOperatorControls() { // Intake ======================================== - // m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle in/out + // m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle + // in/out // m_operatorController // When the trigger is pressed, intake a note at a variable speed - // .leftTrigger(0.1) - // .whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) - // .onFalse(Intake.stopRollersCommand()); + // .leftTrigger(0.1) + // .whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) + // .onFalse(Intake.stopRollersCommand()); // m_operatorController // When the trigger is pressed, eject a note at a constant speed - // .rightTrigger(0.1) - // .whileTrue(Intake.ejectNoteCommand()) - // .onFalse(Intake.stopRollersCommand()); + // .rightTrigger(0.1) + // .whileTrue(Intake.ejectNoteCommand()) + // .onFalse(Intake.stopRollersCommand()); // // Shooter ======================================== // m_operatorController // Toggle the elevation of the shooter - // .rightBumper() - // .onTrue(Shooter.toggleElevationCommand()); + // .rightBumper() + // .onTrue(Shooter.toggleElevationCommand()); // m_operatorController // Runs only the shooter motors at a constant speed to score in the amp - // .x() - // .whileTrue(Shooter.startShootingNoteCommand()) - // .onFalse(Shooter.stopMotorsCommand()); + // .x() + // .whileTrue(Shooter.startShootingNoteCommand()) + // .onFalse(Shooter.stopMotorsCommand()); // // Combined shooter and intake commands =========== // m_operatorController // score in speaker - // .b() - // .onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); + // .b() + // .onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); // m_operatorController // Run sequence to load a note into the shooter for scoring in the amp - // .y() - // .onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); + // .y() + // .onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/DriverDashboard.java b/src/main/java/frc/robot/DriverDashboard.java index 3d92606..720ba38 100644 --- a/src/main/java/frc/robot/DriverDashboard.java +++ b/src/main/java/frc/robot/DriverDashboard.java @@ -19,78 +19,48 @@ public class DriverDashboard { public static UsbCamera m_frontColorCam; public static SendableChooser AutoChooser; - public static GenericEntry AllianceBox = DriverTab - .add("Alliance", false) - .withPosition(15, 0) - .withSize(2, 3) - .withWidget(BuiltInWidgets.kBooleanBox) - .withProperties(Map.of("Color when true", "#FF0000", "Color when false", "#0000FF")) - .getEntry(); + public static GenericEntry AllianceBox = + DriverTab.add("Alliance", false).withPosition(15, 0).withSize(2, 3).withWidget(BuiltInWidgets.kBooleanBox) + .withProperties(Map.of("Color when true", "#FF0000", "Color when false", "#0000FF")).getEntry(); // Drive public static Field2d FieldWidget = new Field2d(); - public static GenericEntry HeadingGyro = DriverTab - .add("Current Heading", 0) - .withWidget(BuiltInWidgets.kGyro) - .withPosition(12, 0) - .withSize(3, 3) - .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) - .getEntry(); - public static GenericEntry RearApTagIdField = DriverTab - .add("Rear APTag", 0) - .withWidget(BuiltInWidgets.kTextView) - .withPosition(12, 3) - .withSize(2, 1) - .getEntry(); - public static GenericEntry FrontApTagIdField = DriverTab - .add("Front APTag", 0) - .withWidget(BuiltInWidgets.kTextView) - .withPosition(14, 3) - .withSize(2, 1) - .getEntry(); - public static GenericEntry RearApTagOffsetDial = DriverTab - .add("Rear APTag X Offset", 0) - .withWidget(BuiltInWidgets.kDial) - .withProperties(Map.of("Min", -29.8, "Max", 29.8)) - .withPosition(12, 4) - .withSize(2, 3) - .getEntry(); - public static GenericEntry FrontPoseEstimationSwitch = DriverTab - .add("F Pose Est.", true) - .withWidget(BuiltInWidgets.kToggleSwitch) - .withPosition(14, 4) - .withSize(2, 1) - .getEntry(); - public static GenericEntry RearPoseEstimationSwitch = DriverTab - .add("R Pose Est.", true) - .withWidget(BuiltInWidgets.kToggleSwitch) - .withPosition(14, 5) - .withSize(2, 1) - .getEntry(); + public static GenericEntry HeadingGyro = + DriverTab.add("Current Heading", 0).withWidget(BuiltInWidgets.kGyro).withPosition(12, 0).withSize(3, 3) + .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) + .getEntry(); + public static GenericEntry RearApTagIdField = + DriverTab.add("Rear APTag", 0).withWidget(BuiltInWidgets.kTextView).withPosition(12, 3).withSize(2, 1).getEntry(); + public static GenericEntry FrontApTagIdField = DriverTab.add("Front APTag", 0).withWidget(BuiltInWidgets.kTextView) + .withPosition(14, 3).withSize(2, 1).getEntry(); + public static GenericEntry RearApTagOffsetDial = + DriverTab.add("Rear APTag X Offset", 0).withWidget(BuiltInWidgets.kDial) + .withProperties(Map.of("Min", -29.8, "Max", 29.8)).withPosition(12, 4).withSize(2, 3).getEntry(); + public static GenericEntry FrontPoseEstimationSwitch = DriverTab.add("F Pose Est.", true) + .withWidget(BuiltInWidgets.kToggleSwitch).withPosition(14, 4).withSize(2, 1).getEntry(); + public static GenericEntry RearPoseEstimationSwitch = DriverTab.add("R Pose Est.", true) + .withWidget(BuiltInWidgets.kToggleSwitch).withPosition(14, 5).withSize(2, 1).getEntry(); // Climbers - public static GenericEntry ClimberControlsActiveBox = DriverTab - .add("Climbers Enabled", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .withPosition(5, 6) - .withSize(3, 2) - .getEntry(); + public static GenericEntry ClimberControlsActiveBox = DriverTab.add("Climbers Enabled", false) + .withWidget(BuiltInWidgets.kBooleanBox).withPosition(5, 6).withSize(3, 2).getEntry(); /** * Constructs a new DriverDashboard and adds complex widgets that must be created in the constructor + * * @param config */ public static void init(boolean isReal) { // DriverTab - // .addCamera( - // "Rear Limelight", - // "Limelight Rear", - // "http://" + config.Drivetrain.LimelightRearName + ".local:5800/stream.mjpg" - // ) - // .withPosition(0, 0) - // .withSize(6, 6) - // .withWidget(BuiltInWidgets.kCameraStream) - // .withProperties(Map.of("Show controls", false, "Show crosshair", false)); + // .addCamera( + // "Rear Limelight", + // "Limelight Rear", + // "http://" + config.Drivetrain.LimelightRearName + ".local:5800/stream.mjpg" + // ) + // .withPosition(0, 0) + // .withSize(6, 6) + // .withWidget(BuiltInWidgets.kCameraStream) + // .withProperties(Map.of("Show controls", false, "Show crosshair", false)); if (isReal) { m_frontColorCam = CameraServer.startAutomaticCapture(); m_frontColorCam.setResolution(320, 240); @@ -98,17 +68,18 @@ public static void init(boolean isReal) { m_frontColorCam.setPixelFormat(PixelFormat.kMJPEG); } // DriverTab - // .add(m_frontColorCam) - // .withPosition(6, 0) - // .withSize(6, 6) - // .withWidget(BuiltInWidgets.kCameraStream) - // .withProperties(Map.of("Show controls", false, "Show crosshair", false)); + // .add(m_frontColorCam) + // .withPosition(6, 0) + // .withSize(6, 6) + // .withWidget(BuiltInWidgets.kCameraStream) + // .withProperties(Map.of("Show controls", false, "Show crosshair", false)); DriverTab.add("Field", FieldWidget).withWidget(BuiltInWidgets.kField).withPosition(0, 0).withSize(12, 6); } /** * Adds an auto chooser to the Shuffleboard and configures it + * * @param chooser */ public static void addAutoChooser(SendableChooser chooser) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c062b2a..d181ee6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,11 +58,9 @@ public Robot() { // Schedule the LED patterns to run at 200Hz // This is the recommended way to schedule a periodic task that runs faster than the robot loop - // This also adds a delay of 3ms to the task to ensure it does not run at the same time as + // This also adds a delay of 3ms to the task to ensure it does not run at the same time as // subsystem periodic functions - addPeriodic(m_robotContainer.LEDs::updateLedStrip, - Units.Milliseconds.of(5), - Units.Milliseconds.of(3)); + addPeriodic(m_robotContainer.LEDs::updateLedStrip, Units.Milliseconds.of(5), Units.Milliseconds.of(3)); } @Override @@ -74,8 +72,8 @@ public void disabledInit() { } /** - * This function is called every robot packet, no matter the mode. Use this for - * things that you want ran during all modes. + * This function is called every robot packet, no matter the mode. Use this for things that you want ran during all + * modes. */ @Override public void robotPeriodic() { @@ -90,13 +88,9 @@ public void robotPeriodic() { @Override public void autonomousInit() { var autoPattern = LEDPattern.gradient(GradientType.kDiscontinuous, getAllianceColor(), Color.kBlack) - .offsetBy(-PwmLEDs.VMap.PixelsPerStrip / 2) - .scrollAtRelativeSpeed(Units.Hertz.of(2)) - .reversed(); + .offsetBy(-PwmLEDs.VMap.PixelsPerStrip / 2).scrollAtRelativeSpeed(Units.Hertz.of(2)).reversed(); var combinedPattern = LEDPattern.gradient(GradientType.kDiscontinuous, getAllianceColor(), Color.kBlack) - .offsetBy(PwmLEDs.VMap.PixelsPerStrip / 2) - .scrollAtRelativeSpeed(Units.Hertz.of(2)) - .blend(autoPattern); + .offsetBy(PwmLEDs.VMap.PixelsPerStrip / 2).scrollAtRelativeSpeed(Units.Hertz.of(2)).blend(autoPattern); m_robotContainer.LEDs.setBackgroundPattern(combinedPattern); m_robotContainer.LEDs.clearForegroundPattern(); @@ -121,7 +115,8 @@ public void autonomousInit() { m_robotContainer.Drivetrain.EstimatePoseUsingFrontCamera = true; m_robotContainer.Drivetrain.EstimatePoseUsingRearCamera = true; - if (onRedAlliance()) m_robotContainer.Drivetrain.resetGyro(); + if (onRedAlliance()) + m_robotContainer.Drivetrain.resetGyro(); SmartDashboard.putString("Robot/Auto/CommandName", m_autonomousCommand.getName()); m_autonomousCommand.schedule(); @@ -176,9 +171,7 @@ public static Color getAllianceColor() { var alliance = DriverStation.getAlliance(); Color allianceColor = Color.kGhostWhite; if (alliance.isPresent()) - allianceColor = alliance.get() == Alliance.Red - ? Color.kRed - : Color.kBlue; + allianceColor = alliance.get() == Alliance.Red ? Color.kRed : Color.kBlue; return allianceColor; } diff --git a/src/main/java/frc/robot/subsystems/PwmLEDs.java b/src/main/java/frc/robot/subsystems/PwmLEDs.java index eb0a98d..1ebf38b 100644 --- a/src/main/java/frc/robot/subsystems/PwmLEDs.java +++ b/src/main/java/frc/robot/subsystems/PwmLEDs.java @@ -15,100 +15,97 @@ @Logged(strategy = Logged.Strategy.OPT_IN) public class PwmLEDs extends SubsystemBase { - public static class VMap { - public static final int PwmPort = 9; - public static final int PixelsPerStrip = 78; - public static final double BackgroundDimAmount = 0.5; - } - - private AddressableLED m_led; - private AddressableLEDBuffer m_ledBuffer; - @Logged(name = "LoopErrorCounter", importance = Logged.Importance.CRITICAL) - public byte _loopErrorCounter = 0; - - private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGhostWhite) - .breathe(Units.Seconds.of(4)); - private LEDPattern m_foregroundPattern = null; - - private Alert m_loopStoppedAlert; - private Dimensionless m_backgroundDimAmount = Units.Percent.of(50); - - public PwmLEDs() { - // Initialize the LED strip and buffer - m_ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); - m_led = new AddressableLED(VMap.PwmPort); - m_led.setLength(m_ledBuffer.getLength()); - m_led.start(); - - // Apply a default pattern to the LED strip + public static class VMap { + public static final int PwmPort = 9; + public static final int PixelsPerStrip = 78; + public static final double BackgroundDimAmount = 0.5; + } + + private AddressableLED m_led; + private AddressableLEDBuffer m_ledBuffer; + @Logged(name = "LoopErrorCounter", importance = Logged.Importance.CRITICAL) + public byte _loopErrorCounter = 0; + + private LEDPattern m_backgroundPattern = LEDPattern.solid(Color.kGhostWhite).breathe(Units.Seconds.of(4)); + private LEDPattern m_foregroundPattern = null; + + private Alert m_loopStoppedAlert; + private Dimensionless m_backgroundDimAmount = Units.Percent.of(50); + + public PwmLEDs() { + // Initialize the LED strip and buffer + m_ledBuffer = new AddressableLEDBuffer(VMap.PixelsPerStrip); + m_led = new AddressableLED(VMap.PwmPort); + m_led.setLength(m_ledBuffer.getLength()); + m_led.start(); + + // Apply a default pattern to the LED strip + m_backgroundPattern.applyTo(m_ledBuffer); + + // Setup the warning for when the loop stops + m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); + m_loopStoppedAlert.set(false); + } + + public void updateLedStrip() { + // If we're not running on a real robot, or we've failed too many times, do nothing. + if (_loopErrorCounter > 3) + return; + + try { + if (m_foregroundPattern == null) { + // If we're only using a background pattern, apply it directly m_backgroundPattern.applyTo(m_ledBuffer); - - // Setup the warning for when the loop stops - m_loopStoppedAlert = new Alert("[LEDs:ERROR] LED update loop failed.", Alert.AlertType.kWarning); - m_loopStoppedAlert.set(false); + } else { + // If we have a foreground pattern, overlay it on a dimmed background + var dimmedBackground = m_backgroundPattern.atBrightness(m_backgroundDimAmount); + m_foregroundPattern.overlayOn(dimmedBackground).applyTo(m_ledBuffer); + } + + // Update the LED strip with the new data + m_led.setData(m_ledBuffer); + } catch (Exception e) { + // If we fail to update the LEDs, report the error and increment the error counter + _loopErrorCounter++; + DataLogManager.log("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage()); + DriverStation.reportError("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage(), e.getStackTrace()); + + // If we've failed too many times, stop the loop and alert the user + if (_loopErrorCounter > 3) { + var msg = "[LEDs:ERROR] LED update loop has failed 3 times. Stopping loop."; + DriverStation.reportError(msg, false); + System.out.println(msg); + m_loopStoppedAlert.set(true); + } } + } - public void updateLedStrip() { - // If we're not running on a real robot, or we've failed too many times, do nothing. - if (_loopErrorCounter > 3) - return; - - try { - if (m_foregroundPattern == null) { - // If we're only using a background pattern, apply it directly - m_backgroundPattern.applyTo(m_ledBuffer); - } else { - // If we have a foreground pattern, overlay it on a dimmed background - var dimmedBackground = m_backgroundPattern.atBrightness(m_backgroundDimAmount); - m_foregroundPattern.overlayOn(dimmedBackground).applyTo(m_ledBuffer); - } - - // Update the LED strip with the new data - m_led.setData(m_ledBuffer); - } catch (Exception e) { - // If we fail to update the LEDs, report the error and increment the error counter - _loopErrorCounter++; - DataLogManager.log("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage()); - DriverStation.reportError("[LEDs:ERROR] Failed to update LEDs: " + e.getMessage(), e.getStackTrace()); - - // If we've failed too many times, stop the loop and alert the user - if (_loopErrorCounter > 3) { - var msg = "[LEDs:ERROR] LED update loop has failed 3 times. Stopping loop."; - DriverStation.reportError(msg, false); - System.out.println(msg); - m_loopStoppedAlert.set(true); - } - } + public void setBackgroundPattern(LEDPattern backgroundPattern) { + if (m_backgroundPattern != backgroundPattern) { + m_backgroundPattern = backgroundPattern; } + } - public void setBackgroundPattern(LEDPattern backgroundPattern) { - if (m_backgroundPattern != backgroundPattern) { - m_backgroundPattern = backgroundPattern; - } + public void setForegroundPattern(LEDPattern foregroundPattern) { + if (DriverStation.isEnabled() && m_foregroundPattern != foregroundPattern) { + m_foregroundPattern = foregroundPattern; } + } - public void setForegroundPattern(LEDPattern foregroundPattern) { - if (DriverStation.isEnabled() && m_foregroundPattern != foregroundPattern) { - m_foregroundPattern = foregroundPattern; - } - } - - public void clearForegroundPattern() { - if (m_foregroundPattern != null) - m_foregroundPattern = null; - } + public void clearForegroundPattern() { + if (m_foregroundPattern != null) + m_foregroundPattern = null; + } - public Command setBackgroundPatternCommand(LEDPattern backgroundPattern) { - return runOnce(() -> setBackgroundPattern(backgroundPattern)) - .ignoringDisable(true); - } + public Command setBackgroundPatternCommand(LEDPattern backgroundPattern) { + return runOnce(() -> setBackgroundPattern(backgroundPattern)).ignoringDisable(true); + } - public Command setForegroundPatternCommand(LEDPattern foregroundPattern) { - return runOnce(() -> setForegroundPattern(foregroundPattern)) - .ignoringDisable(true); // (only allowed when enabled) - } + public Command setForegroundPatternCommand(LEDPattern foregroundPattern) { + return runOnce(() -> setForegroundPattern(foregroundPattern)).ignoringDisable(true); // (only allowed when enabled) + } - public Command clearForegroundPatternCommand() { - return runOnce(() -> clearForegroundPattern()).ignoringDisable(true); - } + public Command clearForegroundPatternCommand() { + return runOnce(() -> clearForegroundPattern()).ignoringDisable(true); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java index 1945ff8..bc393ac 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOInputs.java @@ -4,15 +4,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOInputs; @Logged public class DrivetrainIOInputs { - public Rotation2d GyroAngle = new Rotation2d(); - public double GyroAccelX = 0; - public double GyroAccelY = 0; - public double GyroAccelZ = 0; - public boolean SnapIsOnTarget = false; - public ChassisSpeeds RobotRelativeChassisSpeeds = new ChassisSpeeds(); - public Pose2d EstimatedRobotPose = new Pose2d(); - public double SnapCorrectionRadiansPerSecond = 0; + public Rotation2d GyroAngle = new Rotation2d(); + public double GyroAccelX = 0; + public double GyroAccelY = 0; + public double GyroAccelZ = 0; + public boolean SnapIsOnTarget = false; + public ChassisSpeeds RobotRelativeChassisSpeeds = new ChassisSpeeds(); + public Pose2d EstimatedRobotPose = new Pose2d(); + public double SnapCorrectionRadiansPerSecond = 0; + public SwerveModuleIOInputs[] ModuleInputs = new SwerveModuleIOInputs[4]; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java index 218ff01..f317534 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOOutputs.java @@ -7,8 +7,8 @@ @Logged public class DrivetrainIOOutputs { - public boolean SnapEnabled = false; - public Rotation2d SnapSetpoint = new Rotation2d(); - public ChassisSpeeds DesiredChassisSpeeds = new ChassisSpeeds(); - public DrivetrainControlMode ControlMode = DrivetrainControlMode.kRobotRelative; + public boolean SnapEnabled = false; + public Rotation2d SnapSetpoint = new Rotation2d(); + public ChassisSpeeds DesiredChassisSpeeds = new ChassisSpeeds(); + public DrivetrainControlMode ControlMode = DrivetrainControlMode.kRobotRelative; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java index 9039d88..b2febd3 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOReal.java @@ -43,7 +43,7 @@ public class DrivetrainIOReal implements IDrivetrainIO { private SwerveModuleIOInputs m_rearLeftInputs; @Logged(name = "RearRightInputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOInputs m_rearRightInputs; - + @Logged(name = "FrontLeftOutputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOOutputs m_frontLeftOutputs; @Logged(name = "FrontRightOutputs", importance = Logged.Importance.CRITICAL) @@ -71,17 +71,13 @@ public DrivetrainIOReal() { m_snapAngleController.enableContinuousInput(-Math.PI, Math.PI); // Create kinematics in order FL, FR, RL, RR - m_kinematics = - new SwerveDriveKinematics( - DriveMap.FrontLeftSwerveModule.ModuleLocation, - DriveMap.FrontRightSwerveModule.ModuleLocation, - DriveMap.RearLeftSwerveModule.ModuleLocation, - DriveMap.RearRightSwerveModule.ModuleLocation - ); + m_kinematics = new SwerveDriveKinematics(DriveMap.FrontLeftSwerveModule.ModuleLocation, + DriveMap.FrontRightSwerveModule.ModuleLocation, DriveMap.RearLeftSwerveModule.ModuleLocation, + DriveMap.RearRightSwerveModule.ModuleLocation); // Create pose estimator m_poseEstimator = - new SwerveDrivePoseEstimator(m_kinematics, m_gyro.getRotation2d(), getModulePositions(), new Pose2d()); + new SwerveDrivePoseEstimator(m_kinematics, m_gyro.getRotation2d(), getModulePositions(), new Pose2d()); } @Override @@ -90,6 +86,9 @@ public DrivetrainIOInputs getInputs() { m_frontRightInputs = m_frontRightModule.getInputs(); m_rearLeftInputs = m_rearLeftModule.getInputs(); m_rearRightInputs = m_rearRightModule.getInputs(); + m_inputs.ModuleInputs = new SwerveModuleIOInputs[] { + m_frontLeftInputs, m_frontRightInputs, m_rearLeftInputs, m_rearRightInputs + }; m_inputs.GyroAngle = m_gyro.getRotation2d(); m_inputs.GyroAccelX = m_gyro.getAccelerationX().getValueAsDouble(); @@ -164,10 +163,12 @@ public void stopAllMotors() { /** * Drives robot-relative using a ChassisSpeeds + * * @param desiredChassisSpeeds The desired speeds of the robot */ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snapAngleEnabled) { - // If snap-to is enabled, calculate and override the input rotational speed to reach the setpoint + // If snap-to is enabled, calculate and override the input rotational speed to reach the + // setpoint if (snapAngleEnabled) { var currentRotationRadians = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians()); var snapCorrection = m_snapAngleController.calculate(currentRotationRadians); @@ -178,7 +179,8 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snap m_inputs.SnapIsOnTarget = Math.abs(desiredChassisSpeeds.omegaRadiansPerSecond) < 0.1; } - // Correct drift by taking the input speeds and converting them to a desired per-period speed. This is known as "discretizing" + // Correct drift by taking the input speeds and converting them to a desired per-period speed. + // This is known as "discretizing" desiredChassisSpeeds = ChassisSpeeds.discretize(desiredChassisSpeeds, 0.02); // Calculate the module states from the chassis speeds @@ -191,6 +193,7 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snap /** * Facilitates driving using PathPlanner generated speeds + * * @param robotRelativeSpeeds */ private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { @@ -210,6 +213,7 @@ private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { /** * Sets the desired states for each swerve module in order FL, FR, RL, RR + * * @param desiredStates */ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { @@ -224,10 +228,8 @@ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { */ public SwerveModuleState[] getModuleStates() { return new SwerveModuleState[] { - m_frontLeftInputs.ModuleState, - m_frontRightInputs.ModuleState, - m_rearLeftInputs.ModuleState, - m_rearRightInputs.ModuleState, + m_frontLeftInputs.ModuleState, m_frontRightInputs.ModuleState, m_rearLeftInputs.ModuleState, + m_rearRightInputs.ModuleState, }; } @@ -236,10 +238,8 @@ public SwerveModuleState[] getModuleStates() { */ public SwerveModulePosition[] getModulePositions() { return new SwerveModulePosition[] { - m_frontLeftInputs.ModulePosition, - m_frontRightInputs.ModulePosition, - m_rearLeftInputs.ModulePosition, - m_rearRightInputs.ModulePosition, + m_frontLeftInputs.ModulePosition, m_frontRightInputs.ModulePosition, m_rearLeftInputs.ModulePosition, + m_rearRightInputs.ModulePosition, }; } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java index 4c918a5..f4763aa 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainIOSim.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.drivetrain; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -20,7 +19,6 @@ import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOOutputs; import frc.robot.subsystems.drivetrain.swervemodule.SwerveModuleIOSim; -@Logged(strategy = Strategy.OPT_IN) public class DrivetrainIOSim implements IDrivetrainIO { private DrivetrainIOInputs m_inputs; @@ -33,18 +31,12 @@ public class DrivetrainIOSim implements IDrivetrainIO { @Logged(name = "ModuleInputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOInputs[] m_moduleInputs = new SwerveModuleIOInputs[] { - new SwerveModuleIOInputs(), - new SwerveModuleIOInputs(), - new SwerveModuleIOInputs(), - new SwerveModuleIOInputs() + new SwerveModuleIOInputs(), new SwerveModuleIOInputs(), new SwerveModuleIOInputs(), new SwerveModuleIOInputs() }; @Logged(name = "ModuleOutputs", importance = Logged.Importance.CRITICAL) private SwerveModuleIOOutputs[] m_moduleOutputs = new SwerveModuleIOOutputs[] { - new SwerveModuleIOOutputs(), - new SwerveModuleIOOutputs(), - new SwerveModuleIOOutputs(), - new SwerveModuleIOOutputs() + new SwerveModuleIOOutputs(), new SwerveModuleIOOutputs(), new SwerveModuleIOOutputs(), new SwerveModuleIOOutputs() }; public DrivetrainIOSim() { @@ -59,18 +51,13 @@ public DrivetrainIOSim() { m_rearRightModule = new SwerveModuleIOSim(DriveMap.RearRightSwerveModule); // Create kinematics in order FL, FR, RL, RR - m_kinematics = - new SwerveDriveKinematics( - DriveMap.FrontLeftSwerveModule.ModuleLocation, - DriveMap.FrontRightSwerveModule.ModuleLocation, - DriveMap.RearLeftSwerveModule.ModuleLocation, - DriveMap.RearRightSwerveModule.ModuleLocation - ); + m_kinematics = new SwerveDriveKinematics(DriveMap.FrontLeftSwerveModule.ModuleLocation, + DriveMap.FrontRightSwerveModule.ModuleLocation, DriveMap.RearLeftSwerveModule.ModuleLocation, + DriveMap.RearRightSwerveModule.ModuleLocation); // Create pose estimator - m_poseEstimator = - new SwerveDrivePoseEstimator(m_kinematics, Rotation2d.fromDegrees(m_gyroSim.getAngle()), - getModulePositions(), new Pose2d()); + m_poseEstimator = new SwerveDrivePoseEstimator(m_kinematics, Rotation2d.fromDegrees(m_gyroSim.getAngle()), + getModulePositions(), new Pose2d()); } @Override @@ -79,9 +66,12 @@ public DrivetrainIOInputs getInputs() { m_moduleInputs[1] = m_frontRightModule.getInputs(); m_moduleInputs[2] = m_rearLeftModule.getInputs(); m_moduleInputs[3] = m_rearRightModule.getInputs(); + m_inputs.ModuleInputs = m_moduleInputs; m_inputs.RobotRelativeChassisSpeeds = m_kinematics.toChassisSpeeds(getModuleStates()); - m_gyroSim.setAngle(new Rotation2d(Rotation2d.fromDegrees(m_gyroSim.getAngle()).getRadians() + m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond * 0.02).getDegrees()); + var currentRotationRadians = Rotation2d.fromDegrees(m_gyroSim.getAngle()).getRadians(); + var discretizedOmegaSpeed = m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond * 0.02; + m_gyroSim.setAngle(new Rotation2d(currentRotationRadians + discretizedOmegaSpeed).getDegrees()); var modulePositions = getModulePositions(); m_inputs.EstimatedRobotPose = m_poseEstimator.update(m_inputs.GyroAngle, modulePositions); @@ -95,7 +85,7 @@ public DrivetrainIOInputs getInputs() { public void setOutputs(DrivetrainIOOutputs outputs) { if (outputs.SnapEnabled) { // Set GyroSim to the snap angle with no simulated delays. - m_gyroSim.setAngle(outputs.SnapSetpoint.getDegrees()); + m_gyroSim.setAngle(outputs.SnapSetpoint.getDegrees()); } switch (outputs.ControlMode) { @@ -119,8 +109,8 @@ public void setOutputs(DrivetrainIOOutputs outputs) { public void resetGyro() { m_gyroSim.setAngle(Robot.onBlueAlliance() ? 180 : 0); - m_poseEstimator.resetPosition(Rotation2d.fromDegrees(m_gyroSim.getAngle()), - getModulePositions(), m_poseEstimator.getEstimatedPosition()); + m_poseEstimator.resetPosition(Rotation2d.fromDegrees(m_gyroSim.getAngle()), getModulePositions(), + m_poseEstimator.getEstimatedPosition()); } @Override @@ -143,16 +133,19 @@ public void stopAllMotors() { /** * Drives robot-relative using a ChassisSpeeds + * * @param desiredChassisSpeeds The desired speeds of the robot */ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snapAngleEnabled) { - // If snap-to is enabled, calculate and override the input rotational speed to reach the setpoint + // If snap-to is enabled, calculate and override the input rotational speed to reach the + // setpoint if (snapAngleEnabled) { // Report back that snap is on-target since we're assuming there is no loss in the simulation m_inputs.SnapIsOnTarget = true; } - // Correct drift by taking the input speeds and converting them to a desired per-period speed. This is known as "discretizing" + // Correct drift by taking the input speeds and converting them to a desired per-period speed. + // This is known as "discretizing" desiredChassisSpeeds = ChassisSpeeds.discretize(desiredChassisSpeeds, 0.02); // Calculate the module states from the chassis speeds @@ -165,6 +158,7 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds, boolean snap /** * Facilitates driving using PathPlanner generated speeds + * * @param robotRelativeSpeeds */ private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { @@ -184,6 +178,7 @@ private void drivePathPlanner(ChassisSpeeds robotRelativeSpeeds) { /** * Sets the desired states for each swerve module in order FL, FR, RL, RR + * * @param desiredStates */ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { @@ -198,10 +193,8 @@ private void setDesiredModuleStates(SwerveModuleState[] desiredStates) { */ public SwerveModuleState[] getModuleStates() { return new SwerveModuleState[] { - m_moduleInputs[0].ModuleState, - m_moduleInputs[1].ModuleState, - m_moduleInputs[2].ModuleState, - m_moduleInputs[3].ModuleState, + m_moduleInputs[0].ModuleState, m_moduleInputs[1].ModuleState, m_moduleInputs[2].ModuleState, + m_moduleInputs[3].ModuleState, }; } @@ -210,10 +203,8 @@ public SwerveModuleState[] getModuleStates() { */ public SwerveModulePosition[] getModulePositions() { return new SwerveModulePosition[] { - m_moduleInputs[0].ModulePosition, - m_moduleInputs[1].ModulePosition, - m_moduleInputs[2].ModulePosition, - m_moduleInputs[3].ModulePosition, + m_moduleInputs[0].ModulePosition, m_moduleInputs[1].ModulePosition, m_moduleInputs[2].ModulePosition, + m_moduleInputs[3].ModulePosition, }; } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java index 2866873..ba081d6 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSubsystem.java @@ -29,7 +29,6 @@ import java.util.Map; import java.util.function.Consumer; -import java.util.function.Function; import java.util.function.Supplier; import prime.control.SwerveControlSuppliers; @@ -39,9 +38,7 @@ public class DrivetrainSubsystem extends SubsystemBase { public enum DrivetrainControlMode { - kRobotRelative, - kFieldRelative, - kPathFollowing, + kRobotRelative, kFieldRelative, kPathFollowing, } private Runnable m_clearForegroundPatternFunc; @@ -49,25 +46,15 @@ public enum DrivetrainControlMode { // Shuffleboard Drivetrain tab configuration private ShuffleboardTab d_drivetrainTab = Shuffleboard.getTab("Drivetrain"); - public GenericEntry d_snapToEnabledEntry = d_drivetrainTab - .add("SnapTo Enabled", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .withPosition(0, 0) - .withSize(2, 2) - .getEntry(); - private GenericEntry d_snapAngle = d_drivetrainTab - .add("SnapTo Angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .withPosition(2, 0) - .withSize(4, 5) - .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) - .getEntry(); + public GenericEntry d_snapToEnabledEntry = d_drivetrainTab.add("SnapTo Enabled", false) + .withWidget(BuiltInWidgets.kBooleanBox).withPosition(0, 0).withSize(2, 2).getEntry(); + private GenericEntry d_snapAngle = + d_drivetrainTab.add("SnapTo Angle", 0).withWidget(BuiltInWidgets.kGyro).withPosition(2, 0).withSize(4, 5) + .withProperties(Map.of("Counter clockwise", true, "Major tick spacing", 45.0, "Minor tick spacing", 15.0)) + .getEntry(); // IO - @Logged(name = "DriveIO", importance = Logged.Importance.CRITICAL) - private IDrivetrainIO m_driveio = Robot.isReal() - ? new DrivetrainIOReal() - : new DrivetrainIOSim(); + private IDrivetrainIO m_driveio = Robot.isReal() ? new DrivetrainIOReal() : new DrivetrainIOSim(); private DrivetrainIOInputs m_inputs; @Logged(name = "DriveIOOutputs", importance = Logged.Importance.CRITICAL) private DrivetrainIOOutputs m_outputs; @@ -81,30 +68,25 @@ public enum DrivetrainControlMode { @Logged(importance = Logged.Importance.CRITICAL) public boolean WithinPoseEstimationVelocity = true; - private LEDPattern m_snapOnTargetPattern = LEDPattern.solid(Color.kGreen) - .blink(Units.Seconds.of(0.1)); - private LEDPattern m_snapOffTargetPattern = LEDPattern.steps(Map.of(0.0, Color.kRed, 0.25, Color.kBlack)) - .scrollAtRelativeSpeed(Units.Hertz.of(2)); + private LEDPattern m_snapOnTargetPattern = LEDPattern.solid(Color.kGreen).blink(Units.Seconds.of(0.1)); + private LEDPattern m_snapOffTargetPattern = + LEDPattern.steps(Map.of(0.0, Color.kRed, 0.25, Color.kBlack)).scrollAtRelativeSpeed(Units.Hertz.of(2)); /** * Creates a new Drivetrain. */ - public DrivetrainSubsystem( - boolean isReal, - Runnable clearForegroundPatternFunc, - Consumer setForegroundPatternFunc, - Supplier limelightInputsSupplier) { + public DrivetrainSubsystem(boolean isReal, Runnable clearForegroundPatternFunc, + Consumer setForegroundPatternFunc, Supplier limelightInputsSupplier) { setName("Drivetrain"); m_clearForegroundPatternFunc = clearForegroundPatternFunc; m_setForegroundPatternFunc = setForegroundPatternFunc; + m_limelightInputsSupplier = limelightInputsSupplier; // Create IO m_inputs = m_driveio.getInputs(); m_outputs = new DrivetrainIOOutputs(); - m_limelightInputsSupplier = limelightInputsSupplier; - configurePathPlanner(); } @@ -121,35 +103,27 @@ private void configurePathPlanner() { // Set up PP to feed current path poses to the field widget PathPlannerLogging.setLogCurrentPoseCallback(pose -> DriverDashboard.FieldWidget.setRobotPose(pose)); - PathPlannerLogging.setLogTargetPoseCallback(pose -> - DriverDashboard.FieldWidget.getObject("target pose").setPose(pose) - ); + PathPlannerLogging + .setLogTargetPoseCallback(pose -> DriverDashboard.FieldWidget.getObject("target pose").setPose(pose)); PathPlannerLogging.setLogActivePathCallback(poses -> DriverDashboard.FieldWidget.getObject("path").setPoses(poses)); // Configure PathPlanner holonomic control - AutoBuilder.configure( - () -> m_inputs.EstimatedRobotPose, - m_driveio::setEstimatorPose, - () -> m_inputs.RobotRelativeChassisSpeeds, - (speeds, feedForwards) -> drivePathPlanner(speeds), - new PPHolonomicDriveController( - DriveMap.PathingTranslationPid.toPIDConstants(), - DriveMap.PathingRotationPid.toPIDConstants() - ), - config, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - var alliance = DriverStation.getAlliance(); - - return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; - }, - this - ); + AutoBuilder.configure(() -> m_inputs.EstimatedRobotPose, m_driveio::setEstimatorPose, + () -> m_inputs.RobotRelativeChassisSpeeds, (speeds, feedForwards) -> drivePathPlanner(speeds), + new PPHolonomicDriveController(DriveMap.PathingTranslationPid.toPIDConstants(), + DriveMap.PathingRotationPid.toPIDConstants()), + config, () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + var alliance = DriverStation.getAlliance(); + + return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; + }, this); // Example of overriding PathPlanner's rotation feedback - // PPHolonomicDriveController.overrideRotationFeedback(() -> m_inputs.SnapCorrectionRadiansPerSecond); + // PPHolonomicDriveController.overrideRotationFeedback(() -> + // m_inputs.SnapCorrectionRadiansPerSecond); } - //#region Control methods + // #region Control methods // Resets the Gyro public void resetGyro() { @@ -158,6 +132,7 @@ public void resetGyro() { /** * Drives robot-relative using a ChassisSpeeds + * * @param desiredChassisSpeeds The desired speeds of the robot */ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds) { @@ -167,6 +142,7 @@ private void driveRobotRelative(ChassisSpeeds desiredChassisSpeeds) { /** * Drives field-relative using a ChassisSpeeds + * * @param desiredChassisSpeeds The desired field-relative speeds of the robot */ private void driveFieldRelative(ChassisSpeeds desiredChassisSpeeds) { @@ -184,11 +160,14 @@ private void drivePathPlanner(ChassisSpeeds pathSpeeds) { */ private void setSnapToEnabled(boolean enabled) { m_outputs.SnapEnabled = enabled; - if (!enabled) m_clearForegroundPatternFunc.run(); + if (!enabled) { + m_clearForegroundPatternFunc.run(); + } } /** * Sets the snap-to gyro setpoint, converting from degrees to radians + * * @param angle The angle to snap to in degrees */ private void setSnapToSetpoint(double angle) { @@ -203,7 +182,8 @@ private void setSnapToSetpoint(double angle) { */ private void evaluatePoseEstimation(boolean withinTrustedVelocity, int limelightIndex) { - // If we have a valid target and we're moving in a trusted velocity range, update the pose estimator + // If we have a valid target and we're moving in a trusted velocity range, update the pose + // estimator var limelightInputs = m_limelightInputsSupplier.get()[limelightIndex]; var isValidTarget = VisionSubsystem.isAprilTagIdValid(limelightInputs.ApriltagId); if (isValidTarget && withinTrustedVelocity) { @@ -213,7 +193,7 @@ private void evaluatePoseEstimation(boolean withinTrustedVelocity, int limelight } } - //#endregion + // #endregion /** * Updates odometry and any other periodic drivetrain events @@ -224,13 +204,16 @@ public void periodic() { m_inputs = m_driveio.getInputs(); // Pose estimation - WithinPoseEstimationVelocity = - m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond < 0.2 && // 1 rad/s is about 60 degrees/s - m_inputs.RobotRelativeChassisSpeeds.vxMetersPerSecond < 2 && - m_inputs.RobotRelativeChassisSpeeds.vyMetersPerSecond < 2; + WithinPoseEstimationVelocity = m_inputs.RobotRelativeChassisSpeeds.omegaRadiansPerSecond < 0.2 && // 1 rad/s is + // about 60 + // degrees/s + m_inputs.RobotRelativeChassisSpeeds.vxMetersPerSecond < 2 + && m_inputs.RobotRelativeChassisSpeeds.vyMetersPerSecond < 2; EstimatePoseUsingFrontCamera = DriverDashboard.FrontPoseEstimationSwitch.getBoolean(false); - if (EstimatePoseUsingFrontCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 0); + if (EstimatePoseUsingFrontCamera) { + evaluatePoseEstimation(WithinPoseEstimationVelocity, 0); + } // EstimatePoseUsingRearCamera = DriverDashboard.RearPoseEstimationSwitch.getBoolean(false); // if (EstimatePoseUsingRearCamera) evaluatePoseEstimation(WithinPoseEstimationVelocity, 1); @@ -254,70 +237,68 @@ public void periodic() { d_snapAngle.setDouble(m_outputs.SnapSetpoint.getRadians()); } - //#region Commands + // #region Commands /** * Creates a command that drives the robot using input controls + * * @param controlSuppliers Controller input suppliers */ public Command driveFieldRelativeCommand(SwerveControlSuppliers controlSuppliers) { return this.run(() -> { - // If the driver is trying to rotate the robot, disable snap-to control - if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { - setSnapToEnabled(false); - m_clearForegroundPatternFunc.run(); - } - - // Convert inputs to MPS - var inputXMPS = controlSuppliers.X.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; - var inputYMPS = -controlSuppliers.Y.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; - var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; - - // Build chassis speeds - var invert = Robot.onRedAlliance() ? -1 : 1; - - // Drive the robot with the driver-relative inputs, converted to field-relative based on which side we're on - var fieldChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - (inputYMPS * invert), // Use Y as X for field-relative - (inputXMPS * invert), // Use X as Y for field-relative - inputRotationRadiansPS, - m_inputs.GyroAngle - ); - - driveFieldRelative(fieldChassisSpeeds); - }); + // If the driver is trying to rotate the robot, disable snap-to control + if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { + setSnapToEnabled(false); + m_clearForegroundPatternFunc.run(); + } + + // Convert inputs to MPS + var inputXMPS = controlSuppliers.X.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputYMPS = -controlSuppliers.Y.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; + + // Build chassis speeds + var invert = Robot.onRedAlliance() ? -1 : 1; + + // Drive the robot with the driver-relative inputs, converted to field-relative based on which + // side we're on + // Use Y as X for field-relative + var fieldChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds((inputYMPS * invert), (inputXMPS * invert), + inputRotationRadiansPS, m_inputs.GyroAngle); + + driveFieldRelative(fieldChassisSpeeds); + }); } /** * Creates a command that drives the robot using input controls + * * @param controlSuppliers Controller input suppliers */ public Command driveRobotRelativeCommand(SwerveControlSuppliers controlSuppliers) { return this.run(() -> { - // If the driver is trying to rotate the robot, disable snap-to control - if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { - setSnapToEnabled(false); - m_clearForegroundPatternFunc.run(); - } - - // Convert inputs to MPS - var inputXMPS = controlSuppliers.X.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; - var inputYMPS = -controlSuppliers.Y.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; - var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; - - // Build chassis speeds - var invert = Robot.onRedAlliance() ? -1 : 1; - - // Drive the robot with the driver-relative inputs, converted to field-relative based on which side we're on - var robotChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( - (inputYMPS * invert), // Use Y as X for field-relative - (inputXMPS * invert), // Use X as Y for field-relative - inputRotationRadiansPS, - m_inputs.GyroAngle - ); - - driveRobotRelative(robotChassisSpeeds); - }); + // If the driver is trying to rotate the robot, disable snap-to control + if (Math.abs(controlSuppliers.Z.getAsDouble()) > 0.2) { + setSnapToEnabled(false); + m_clearForegroundPatternFunc.run(); + } + + // Convert inputs to MPS + var inputXMPS = controlSuppliers.X.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputYMPS = -controlSuppliers.Y.getAsDouble() * DriveMap.MaxSpeedMetersPerSecond; + var inputRotationRadiansPS = -controlSuppliers.Z.getAsDouble() * DriveMap.MaxAngularSpeedRadians; + + // Build chassis speeds + var invert = Robot.onRedAlliance() ? -1 : 1; + + // Drive the robot with the driver-relative inputs, converted to field-relative based on which + // side we're on + // Use Y as X for field-relative + var robotChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds((inputYMPS * invert), (inputXMPS * invert), + inputRotationRadiansPS, m_inputs.GyroAngle); + + driveRobotRelative(robotChassisSpeeds); + }); } /** @@ -329,6 +310,7 @@ public Command resetGyroCommand() { /** * Enables snap-to control and sets an angle setpoint + * * @param angle */ public Command setSnapToSetpointCommand(double angle) { @@ -367,5 +349,5 @@ public Command enableLockOn() { public Map getNamedCommands() { return Map.of("Enable_Lock_On", enableLockOn(), "Disable_Snap_To", disableSnapToCommand()); } - //#endregion + // #endregion } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java b/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java index 5f18ea4..2c8963d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/IDrivetrainIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.drivetrain; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -9,10 +7,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -@Logged(strategy = Strategy.OPT_IN) public interface IDrivetrainIO { - @Logged(name = "Inputs", importance = Logged.Importance.CRITICAL) public DrivetrainIOInputs getInputs(); public void setOutputs(DrivetrainIOOutputs outputs); @@ -21,13 +17,12 @@ public interface IDrivetrainIO { public void setEstimatorPose(Pose2d pose); - public void addPoseEstimatorVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDeviations); + public void addPoseEstimatorVisionMeasurement(Pose2d pose, double timestamp, + Matrix stdDeviations); public void stopAllMotors(); - @Logged(name = "ModuleStates", importance = Logged.Importance.CRITICAL) public SwerveModuleState[] getModuleStates(); - @Logged(name = "ModulePositions", importance = Logged.Importance.CRITICAL) public SwerveModulePosition[] getModulePositions(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java index 6f39e66..e8c189d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOInputs.java @@ -8,16 +8,16 @@ @Logged public class SwerveModuleIOInputs implements StructSerializable { - /** SwerveModuleIOInputs struct for serialization. */ - public static final SwerveModuleIOInputsStruct struct = new SwerveModuleIOInputsStruct(); + /** SwerveModuleIOInputs struct for serialization. */ + public static final SwerveModuleIOInputsStruct struct = new SwerveModuleIOInputsStruct(); - public SwerveModuleIOInputs() { } + public SwerveModuleIOInputs() {} - public SwerveModuleIOInputs(SwerveModulePosition modulePosition, SwerveModuleState moduleState) { - ModulePosition = modulePosition; - ModuleState = moduleState; - } + public SwerveModuleIOInputs(SwerveModulePosition modulePosition, SwerveModuleState moduleState) { + ModulePosition = modulePosition; + ModuleState = moduleState; + } - public SwerveModulePosition ModulePosition = new SwerveModulePosition(); - public SwerveModuleState ModuleState = new SwerveModuleState(); + public SwerveModulePosition ModulePosition = new SwerveModulePosition(); + public SwerveModuleState ModuleState = new SwerveModuleState(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java index b7912eb..91410f8 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOOutputs.java @@ -8,14 +8,14 @@ @Logged public class SwerveModuleIOOutputs implements StructSerializable { - /** Struct for serialization. */ - public static final SwerveModuleIOOutputsStruct struct = new SwerveModuleIOOutputsStruct(); + /** Struct for serialization. */ + public static final SwerveModuleIOOutputsStruct struct = new SwerveModuleIOOutputsStruct(); - public SwerveModuleIOOutputs() { } + public SwerveModuleIOOutputs() {} - public SwerveModuleIOOutputs(SwerveModuleState desiredState) { - DesiredState = desiredState; - } + public SwerveModuleIOOutputs(SwerveModuleState desiredState) { + DesiredState = desiredState; + } - public SwerveModuleState DesiredState = new SwerveModuleState(); + public SwerveModuleState DesiredState = new SwerveModuleState(); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java index bd4bd30..18ed9d0 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOReal.java @@ -47,16 +47,10 @@ public SwerveModuleIOReal(SwerveModuleMap moduleMap) { @Override public SwerveModuleIOInputs getInputs() { var rotation = Rotation2d.fromRotations(m_encoder.getPosition().getValueAsDouble()); - var speedMps = CTREConverter.rotationsToMeters( - m_driveMotor.getVelocity().getValueAsDouble(), - DriveMap.DriveWheelCircumferenceMeters, - DriveMap.DriveGearRatio - ); - var distanceMeters = CTREConverter.rotationsToMeters( - m_driveMotor.getPosition().getValueAsDouble(), - DriveMap.DriveWheelCircumferenceMeters, - DriveMap.DriveGearRatio - ); + var speedMps = CTREConverter.rotationsToMeters(m_driveMotor.getVelocity().getValueAsDouble(), + DriveMap.DriveWheelCircumferenceMeters, DriveMap.DriveGearRatio); + var distanceMeters = CTREConverter.rotationsToMeters(m_driveMotor.getPosition().getValueAsDouble(), + DriveMap.DriveWheelCircumferenceMeters, DriveMap.DriveGearRatio); m_inputs.ModuleState.angle = rotation; m_inputs.ModuleState.speedMetersPerSecond = speedMps; @@ -97,6 +91,7 @@ private void setupSteeringMotor(PrimePIDConstants pid) { /** * Configures the drive motors + * * @param pid */ private void setupDriveMotor(PrimePIDConstants pid) { @@ -108,7 +103,7 @@ private void setupDriveMotor(PrimePIDConstants pid) { // Set the PID values for slot 0 driveMotorConfig.Slot0 = - new Slot0Configs().withKP(pid.kP).withKI(pid.kI).withKD(pid.kD).withKS(pid.kS).withKV(pid.kV); + new Slot0Configs().withKP(pid.kP).withKI(pid.kI).withKD(pid.kD).withKS(pid.kS).withKV(pid.kV); // Set the voltage limits driveMotorConfig.Voltage.PeakForwardVoltage = 12; @@ -119,9 +114,9 @@ private void setupDriveMotor(PrimePIDConstants pid) { // Set the ramp rates driveMotorConfig.withClosedLoopRamps(m_map.DriveClosedLoopRampConfiguration); - driveMotorConfig.MotorOutput.Inverted = m_map.DriveInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; // Clockwise Inversion + driveMotorConfig.MotorOutput.Inverted = + m_map.DriveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; // Clockwise + // Inversion // Apply the configuration m_driveMotor.getConfigurator().apply(driveMotorConfig); @@ -137,40 +132,31 @@ private void setupCanCoder() { m_encoder.getConfigurator().apply(new CANcoderConfiguration()); // AbsoluteSensorRangeValue - m_encoder - .getConfigurator() - .apply( - new CANcoderConfiguration() - .withMagnetSensor( - new MagnetSensorConfigs() - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - .withMagnetOffset(-m_map.CanCoderStartingOffset) - ) - ); + m_encoder.getConfigurator() + .apply(new CANcoderConfiguration() + .withMagnetSensor(new MagnetSensorConfigs().withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withMagnetOffset(-m_map.CanCoderStartingOffset))); } /** * Sets the desired state of the module. * - * @param desiredState The optimized state of the module that we'd like to be at in this - * period + * @param desiredState The optimized state of the module that we'd like to be at in this period */ private void setDesiredState(SwerveModuleState desiredState) { // Optimize the desired state desiredState = optimize(desiredState); // Set the drive motor to the desired speed - var speedRotationsPerSecond = CTREConverter.metersToRotations( - desiredState.speedMetersPerSecond, - DriveMap.DriveWheelCircumferenceMeters, - DriveMap.DriveGearRatio - ); + var speedRotationsPerSecond = CTREConverter.metersToRotations(desiredState.speedMetersPerSecond, + DriveMap.DriveWheelCircumferenceMeters, DriveMap.DriveGearRatio); m_driveMotor.setControl(m_voltageVelocity.withVelocity(speedRotationsPerSecond)); // Set the steering motor to the desired angle var setpoint = desiredState.angle.getRotations() % 1; - if (setpoint < 0) setpoint += 1; + if (setpoint < 0) + setpoint += 1; var newOutput = m_steeringPidController.calculate(m_inputs.ModuleState.angle.getRotations(), setpoint); @@ -178,17 +164,17 @@ private void setDesiredState(SwerveModuleState desiredState) { } /** - * Optimizes the module angle & drive inversion to ensure the module takes the shortest path to drive at the desired angle + * Optimizes the module angle & drive inversion to ensure the module takes the shortest path to drive at the desired + * angle + * * @param desiredState */ private SwerveModuleState optimize(SwerveModuleState desiredState) { Rotation2d currentAngle = m_inputs.ModulePosition.angle; var delta = desiredState.angle.minus(currentAngle); if (Math.abs(delta.getDegrees()) > 90.0) { - return new SwerveModuleState( - -desiredState.speedMetersPerSecond, - desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)) - ); + return new SwerveModuleState(-desiredState.speedMetersPerSecond, + desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0))); } else { return desiredState; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index e8419a7..baa92d6 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -15,7 +15,6 @@ public class SwerveModuleIOSim implements ISwerveModuleIO { - private SwerveModuleMap m_Map; private SwerveModuleIOInputs m_inputs = new SwerveModuleIOInputs(); // Devices @@ -25,20 +24,20 @@ public class SwerveModuleIOSim implements ISwerveModuleIO { private Rotation2d m_steerAngle = new Rotation2d(); public SwerveModuleIOSim(SwerveModuleMap moduleMap) { - m_Map = moduleMap; - setupDriveMotor(DriveMap.DrivePID); } @Override public SwerveModuleIOInputs getInputs() { m_driveMotorSim.update(0.020); - var speedMps = m_driveMotorSim.getAngularVelocity().in(Units.RotationsPerSecond) * DriveMap.DriveWheelCircumferenceMeters; - + var speedMps = + m_driveMotorSim.getAngularVelocity().in(Units.RotationsPerSecond) * DriveMap.DriveWheelCircumferenceMeters; + m_inputs.ModuleState.angle = m_steerAngle; m_inputs.ModuleState.speedMetersPerSecond = speedMps; m_inputs.ModulePosition.angle = m_steerAngle; - m_inputs.ModulePosition.distanceMeters = m_driveMotorSim.getAngularPositionRotations() * DriveMap.DriveWheelCircumferenceMeters; + m_inputs.ModulePosition.distanceMeters = + m_driveMotorSim.getAngularPositionRotations() * DriveMap.DriveWheelCircumferenceMeters; return m_inputs; } @@ -56,17 +55,13 @@ public void stopMotors() { /** * Configures the drive motors + * * @param pid */ private void setupDriveMotor(PrimePIDConstants pid) { - m_driveMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getFalcon500(1), - 0.001, - DriveMap.DriveGearRatio - ), - DCMotor.getFalcon500(1) - ); + m_driveMotorSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), 0.001, DriveMap.DriveGearRatio), + DCMotor.getFalcon500(1)); m_driveFeedback = new PIDController(0.1, 0, 0); m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.085); @@ -75,8 +70,7 @@ private void setupDriveMotor(PrimePIDConstants pid) { /** * Sets the desired state of the module. * - * @param desiredState The optimized state of the module that we'd like to be at in this - * period + * @param desiredState The optimized state of the module that we'd like to be at in this period */ private void setDesiredState(SwerveModuleState desiredState) { // Optimize the desired state @@ -86,7 +80,7 @@ private void setDesiredState(SwerveModuleState desiredState) { // Calculate target data to voltage data var velocityRadPerSec = desiredState.speedMetersPerSecond / (DriveMap.DriveWheelDiameterMeters / 2); var driveAppliedVolts = m_driveFeedforward.calculate(velocityRadPerSec) - + m_driveFeedback.calculate(m_driveMotorSim.getAngularVelocityRadPerSec(), velocityRadPerSec); + + m_driveFeedback.calculate(m_driveMotorSim.getAngularVelocityRadPerSec(), velocityRadPerSec); driveAppliedVolts = MathUtil.clamp(driveAppliedVolts, -12.0, 12.0); m_driveMotorSim.setInputVoltage(driveAppliedVolts); @@ -95,17 +89,17 @@ private void setDesiredState(SwerveModuleState desiredState) { } /** - * Optimizes the module angle & drive inversion to ensure the module takes the shortest path to drive at the desired angle + * Optimizes the module angle & drive inversion to ensure the module takes the shortest path to drive at the desired + * angle + * * @param desiredState */ private SwerveModuleState optimize(SwerveModuleState desiredState) { Rotation2d currentAngle = m_inputs.ModulePosition.angle; var delta = desiredState.angle.minus(currentAngle); if (Math.abs(delta.getDegrees()) > 90.0) { - return new SwerveModuleState( - -desiredState.speedMetersPerSecond, - desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)) - ); + return new SwerveModuleState(-desiredState.speedMetersPerSecond, + desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0))); } else { return desiredState; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java index 38e5c4f..e63e15b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOInputsStruct.java @@ -9,42 +9,44 @@ public class SwerveModuleIOInputsStruct implements Struct { - @Override - public Class getTypeClass() { - return SwerveModuleIOInputs.class; - } - - @Override - public String getTypeName() { - return SwerveModuleIOInputs.class.getName(); - } - - @Override - public int getSize() { - return SwerveModulePosition.struct.getSize() + SwerveModuleState.struct.getSize(); - } - - @Override - public String getSchema() { - return "SwerveModulePosition ModulePosition;SwerveModuleState ModuleState"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {SwerveModulePosition.struct, SwerveModuleState.struct}; - } - - @Override - public SwerveModuleIOInputs unpack(ByteBuffer bb) { - var position = SwerveModulePosition.struct.unpack(bb); - var state = SwerveModuleState.struct.unpack(bb); - return new SwerveModuleIOInputs(position, state); - } - - @Override - public void pack(ByteBuffer bb, SwerveModuleIOInputs value) { - SwerveModulePosition.struct.pack(bb, value.ModulePosition); - SwerveModuleState.struct.pack(bb, value.ModuleState); - } + @Override + public Class getTypeClass() { + return SwerveModuleIOInputs.class; + } + + @Override + public String getTypeName() { + return SwerveModuleIOInputs.class.getName(); + } + + @Override + public int getSize() { + return SwerveModulePosition.struct.getSize() + SwerveModuleState.struct.getSize(); + } + + @Override + public String getSchema() { + return "SwerveModulePosition ModulePosition;SwerveModuleState ModuleState"; + } + + @Override + public Struct[] getNested() { + return new Struct[] { + SwerveModulePosition.struct, SwerveModuleState.struct + }; + } + + @Override + public SwerveModuleIOInputs unpack(ByteBuffer bb) { + var position = SwerveModulePosition.struct.unpack(bb); + var state = SwerveModuleState.struct.unpack(bb); + return new SwerveModuleIOInputs(position, state); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleIOInputs value) { + SwerveModulePosition.struct.pack(bb, value.ModulePosition); + SwerveModuleState.struct.pack(bb, value.ModuleState); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java index 3c35041..ed2e5d0 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/struct/SwerveModuleIOOutputsStruct.java @@ -8,40 +8,42 @@ public class SwerveModuleIOOutputsStruct implements Struct { - @Override - public Class getTypeClass() { - return SwerveModuleIOOutputs.class; - } - - @Override - public String getTypeName() { - return SwerveModuleIOOutputs.class.getName(); - } - - @Override - public int getSize() { - return SwerveModuleState.struct.getSize(); - } - - @Override - public String getSchema() { - return "SwerveModuleState DesiredState"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {SwerveModuleState.struct}; - } - - @Override - public SwerveModuleIOOutputs unpack(ByteBuffer bb) { - var state = SwerveModuleState.struct.unpack(bb); - return new SwerveModuleIOOutputs(state); - } - - @Override - public void pack(ByteBuffer bb, SwerveModuleIOOutputs value) { - SwerveModuleState.struct.pack(bb, value.DesiredState); - } + @Override + public Class getTypeClass() { + return SwerveModuleIOOutputs.class; + } + + @Override + public String getTypeName() { + return SwerveModuleIOOutputs.class.getName(); + } + + @Override + public int getSize() { + return SwerveModuleState.struct.getSize(); + } + + @Override + public String getSchema() { + return "SwerveModuleState DesiredState"; + } + + @Override + public Struct[] getNested() { + return new Struct[] { + SwerveModuleState.struct + }; + } + + @Override + public SwerveModuleIOOutputs unpack(ByteBuffer bb) { + var state = SwerveModuleState.struct.unpack(bb); + return new SwerveModuleIOOutputs(state); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleIOOutputs value) { + SwerveModuleState.struct.pack(bb, value.DesiredState); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java index 166efc2..5f4a6b3 100644 --- a/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java +++ b/src/main/java/frc/robot/subsystems/vision/LimeLightNT.java @@ -15,277 +15,293 @@ import prime.vision.LimelightPose; public class LimeLightNT implements AutoCloseable { - private NetworkTable m_limelightTable; - private ExecutorService m_executorService = Executors.newSingleThreadExecutor(); - - private LimelightInputs m_inputs = new LimelightInputs(); - - public LimeLightNT(NetworkTableInstance instance, String tableName) { - m_limelightTable = instance.getTable(tableName); - } - - public LimelightInputs getInputs() { - m_inputs.TargetHorizontalOffset = getHorizontalOffsetFromTarget(); - m_inputs.TargetVerticalOffset = getVerticalOffsetFromTarget(); - m_inputs.TargetArea = getTargetArea(); - m_inputs.PipelineLatencyMs = getPipelineLatencyMs(); - m_inputs.CapturePipelineLatencyMs = getCapturePipelineLatencyMs(); - m_inputs.TotalLatencyMs = getTotalLatencyMs(); - m_inputs.ApriltagId = getApriltagId(); - m_inputs.TagCount = getTagCount(); - m_inputs.FieldSpaceRobotPose = getRobotPose(); - m_inputs.RedAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Red); - m_inputs.BlueAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Blue); - - return m_inputs; - } - - //#region Basic Targeting Data - - /** - * Returns Horizontal Offset From Crosshair To Target (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) - */ - public Rotation2d getHorizontalOffsetFromTarget() { - return Rotation2d.fromDegrees(m_limelightTable.getEntry("tx").getDouble(0.0)); - } - - /** - * Returns Vertical Offset From Crosshair To Target (LL1: -20.5 degrees to 20.5 degrees / LL2: -24.85 to 24.85 degrees) - */ - public Rotation2d getVerticalOffsetFromTarget() { - return Rotation2d.fromDegrees(m_limelightTable.getEntry("ty").getDouble(0.0)); - } - - /** - * Returns Target Area (0% of image to 100% of image) - */ - public double getTargetArea() { - return m_limelightTable.getEntry("ta").getDouble(0.0); - } - - /** - * The pipeline's latency contribution (ms). Add to "cl" to get total latency. - */ - public int getPipelineLatencyMs() { - return m_limelightTable.getEntry("tl").getNumber(0).intValue(); - } - - /** - * Capture pipeline latency (ms). Time between the end of the exposure of the middle row of the sensor to the beginning of the tracking pipeline. - */ - public int getCapturePipelineLatencyMs() { - return m_limelightTable.getEntry("cl").getNumber(0).intValue(); - } - - /** - * The total latency of the capture and pipeline processing in milliseconds. - * @return - */ - public int getTotalLatencyMs() { - return getPipelineLatencyMs() + getCapturePipelineLatencyMs(); - } - - //#endregion - - //#region AprilTag and 3D Data - - /** - * ID of the primary in-view AprilTag - */ - public int getApriltagId() { - return m_limelightTable.getEntry("tid").getNumber(0).intValue(); - } - - /** - * Returns the number of AprilTags in the image. - * @return - */ - public double getTagCount() { - // Robot transform in field-space. Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency (cl+tl), tag count, tag span, average tag distance from camera, average tag area (percentage of image) - var botPose = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); - - return botPose[7]; - } - - /** - * Robot transform in field-space. - */ - public LimelightPose getRobotPose() { - var poseData = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * Robot transform in field-space (alliance driverstation WPILIB origin). - * @param alliance - */ - public LimelightPose getRobotPose(DriverStation.Alliance alliance) { - var poseData = alliance == Alliance.Blue - ? m_limelightTable.getEntry("botpose_wpiblue").getDoubleArray(new double[11]) - : m_limelightTable.getEntry("botpose_wpired").getDoubleArray(new double[11]); - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the robot in the coordinate system of the primary in-view AprilTag - */ - public LimelightPose getRobotPoseInTargetSpace() { - var poseData = m_limelightTable.getEntry("botpose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the camera in the coordinate system of the primary in-view AprilTag - */ - public LimelightPose getCameraPoseInTargetSpace() { - var poseData = m_limelightTable.getEntry("camerapose_targetspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the camera in the coordinate system of the robot - */ - public LimelightPose getCameraPoseInRobotSpace() { - var poseData = m_limelightTable.getEntry("camerapose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera - */ - public LimelightPose getTargetPoseInCameraSpace() { - var poseData = m_limelightTable.getEntry("targetpose_cameraspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot - */ - public LimelightPose getTargetPoseInRobotSpace() { - var poseData = m_limelightTable.getEntry("targetpose_robotspace").getDoubleArray(new double[11]); // Translation (X,Y,Z) Rotation(Roll,Pitch,Yaw) - - return new LimelightPose(poseData, calculateTrust(poseData[7])); - } - - /** - * Calculates a trust value based on the number of tags in view. - * @return - */ - public double[] calculateTrust(double tagCount) { - // Trust level is a function of the number of tags in view - // var trustLevel = 0.490956d + Math.pow(9998.51d, -(6.95795d * tagCount)); - - var trustLevel = tagCount >= 2.0 ? 2 : 20; - - // X Y Z trust levels (never trust Z) - return VecBuilder.fill(trustLevel, trustLevel, 999999).getData(); - } - - //#endregion - - //#region Camera Controls - - /** - * Sets limelight’s LED state. - * 0 = use the LED Mode set in the current pipeline. - * 1 = force off. - * 2 = force blink. - * 3 = force on. - * @param mode - */ - public void setLedMode(int mode) { - m_limelightTable.getEntry("ledMode").setNumber(mode); - } - - /** - * Forces the LED to blink a specified number of times, then returns to pipeline control. - */ - public void blinkLed(int blinkCount) { - m_executorService.submit(() -> { - // Blink the LED X times with 100ms on, 200ms off for each blink - for (int i = 0; i < blinkCount; i++) { - m_limelightTable.getEntry("ledMode").setNumber(3); - - try { - Thread.sleep(100); - } catch (Exception e) { - Thread.currentThread().interrupt(); - } - - m_limelightTable.getEntry("ledMode").setNumber(1); - try { - Thread.sleep(200); - } catch (Exception e) { - Thread.currentThread().interrupt(); - } + private NetworkTable m_limelightTable; + private ExecutorService m_executorService = Executors.newSingleThreadExecutor(); + + private LimelightInputs m_inputs = new LimelightInputs(); + + public LimeLightNT(NetworkTableInstance instance, String tableName) { + m_limelightTable = instance.getTable(tableName); + } + + public LimelightInputs getInputs() { + m_inputs.TargetHorizontalOffset = getHorizontalOffsetFromTarget(); + m_inputs.TargetVerticalOffset = getVerticalOffsetFromTarget(); + m_inputs.TargetArea = getTargetArea(); + m_inputs.PipelineLatencyMs = getPipelineLatencyMs(); + m_inputs.CapturePipelineLatencyMs = getCapturePipelineLatencyMs(); + m_inputs.TotalLatencyMs = getTotalLatencyMs(); + m_inputs.ApriltagId = getApriltagId(); + m_inputs.TagCount = getTagCount(); + m_inputs.FieldSpaceRobotPose = getRobotPose(); + m_inputs.RedAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Red); + m_inputs.BlueAllianceOriginFieldSpaceRobotPose = getRobotPose(Alliance.Blue); + + return m_inputs; + } + + // #region Basic Targeting Data + + /** + * Returns Horizontal Offset From Crosshair To Target (LL1: -27 degrees to 27 degrees / LL2: -29.8 to 29.8 degrees) + */ + public Rotation2d getHorizontalOffsetFromTarget() { + return Rotation2d.fromDegrees(m_limelightTable.getEntry("tx").getDouble(0.0)); + } + + /** + * Returns Vertical Offset From Crosshair To Target (LL1: -20.5 degrees to 20.5 degrees / LL2: -24.85 to 24.85 + * degrees) + */ + public Rotation2d getVerticalOffsetFromTarget() { + return Rotation2d.fromDegrees(m_limelightTable.getEntry("ty").getDouble(0.0)); + } + + /** + * Returns Target Area (0% of image to 100% of image) + */ + public double getTargetArea() { + return m_limelightTable.getEntry("ta").getDouble(0.0); + } + + /** + * The pipeline's latency contribution (ms). Add to "cl" to get total latency. + */ + public int getPipelineLatencyMs() { + return m_limelightTable.getEntry("tl").getNumber(0).intValue(); + } + + /** + * Capture pipeline latency (ms). Time between the end of the exposure of the middle row of the sensor to the + * beginning of the tracking pipeline. + */ + public int getCapturePipelineLatencyMs() { + return m_limelightTable.getEntry("cl").getNumber(0).intValue(); + } + + /** + * The total latency of the capture and pipeline processing in milliseconds. + * + * @return + */ + public int getTotalLatencyMs() { + return getPipelineLatencyMs() + getCapturePipelineLatencyMs(); + } + + // #endregion + + // #region AprilTag and 3D Data + + /** + * ID of the primary in-view AprilTag + */ + public int getApriltagId() { + return m_limelightTable.getEntry("tid").getNumber(0).intValue(); + } + + /** + * Returns the number of AprilTags in the image. + * + * @return + */ + public double getTagCount() { + // Robot transform in field-space. Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency + // (cl+tl), tag count, tag span, average tag distance from camera, average tag area (percentage of image) + var botPose = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); + + return botPose[7]; + } + + /** + * Robot transform in field-space. + */ + public LimelightPose getRobotPose() { + var poseData = m_limelightTable.getEntry("botpose").getDoubleArray(new double[11]); // Translation (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * Robot transform in field-space (alliance driverstation WPILIB origin). + * + * @param alliance + */ + public LimelightPose getRobotPose(DriverStation.Alliance alliance) { + var poseData = + alliance == Alliance.Blue ? m_limelightTable.getEntry("botpose_wpiblue").getDoubleArray(new double[11]) + : m_limelightTable.getEntry("botpose_wpired").getDoubleArray(new double[11]); + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the robot in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose getRobotPoseInTargetSpace() { + var poseData = m_limelightTable.getEntry("botpose_targetspace").getDoubleArray(new double[11]); // Translation + // (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the camera in the coordinate system of the primary in-view AprilTag + */ + public LimelightPose getCameraPoseInTargetSpace() { + var poseData = m_limelightTable.getEntry("camerapose_targetspace").getDoubleArray(new double[11]); // Translation + // (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the camera in the coordinate system of the robot + */ + public LimelightPose getCameraPoseInRobotSpace() { + var poseData = m_limelightTable.getEntry("camerapose_robotspace").getDoubleArray(new double[11]); // Translation + // (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Camera + */ + public LimelightPose getTargetPoseInCameraSpace() { + var poseData = m_limelightTable.getEntry("targetpose_cameraspace").getDoubleArray(new double[11]); // Translation + // (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * 3D transform of the primary in-view AprilTag in the coordinate system of the Robot + */ + public LimelightPose getTargetPoseInRobotSpace() { + var poseData = m_limelightTable.getEntry("targetpose_robotspace").getDoubleArray(new double[11]); // Translation + // (X,Y,Z) + // Rotation(Roll,Pitch,Yaw) + + return new LimelightPose(poseData, calculateTrust(poseData[7])); + } + + /** + * Calculates a trust value based on the number of tags in view. + * + * @return + */ + public double[] calculateTrust(double tagCount) { + // Trust level is a function of the number of tags in view + // var trustLevel = 0.490956d + Math.pow(9998.51d, -(6.95795d * tagCount)); + + var trustLevel = tagCount >= 2.0 ? 2 : 20; + + // X Y Z trust levels (never trust Z) + return VecBuilder.fill(trustLevel, trustLevel, 999999).getData(); + } + + // #endregion + + // #region Camera Controls + + /** + * Sets limelight’s LED state. 0 = use the LED Mode set in the current pipeline. 1 = force off. 2 = force blink. 3 = + * force on. + * + * @param mode + */ + public void setLedMode(int mode) { + m_limelightTable.getEntry("ledMode").setNumber(mode); + } + + /** + * Forces the LED to blink a specified number of times, then returns to pipeline control. + */ + public void blinkLed(int blinkCount) { + m_executorService.submit(() -> { + // Blink the LED X times with 100ms on, 200ms off for each blink + for (int i = 0; i < blinkCount; i++) { + m_limelightTable.getEntry("ledMode").setNumber(3); + + try { + Thread.sleep(100); + } catch (Exception e) { + Thread.currentThread().interrupt(); } - // Then return to pipeline control - setLedMode(0); - }); - } - - /** - * Sets limelight’s operation mode. - * 0 = Vision processor. - * 1 = Driver Camera (Increases exposure, disables vision processing). - * @param mode - */ - public void setCameraMode(int mode) { - m_limelightTable.getEntry("camMode").setNumber(mode); - } - - /** - * Sets limelight’s pipeline. - * @param pipeline - */ - public void setPipeline(int pipeline) { - m_limelightTable.getEntry("pipeline").setNumber(pipeline); - } - - /** - * Side-by-side streams (Note: USB output stream is not affected by this mode) - * @param mode - */ - public void setPiPStreamingMode(int mode) { - m_limelightTable.getEntry("stream").setNumber(mode); - } - - /** - * Allows users to take snapshots during a match - */ - public void takeSnapshot() { - m_limelightTable.getEntry("snapshot").setNumber(0); // Reset - m_limelightTable.getEntry("snapshot").setNumber(1); // Take snapshot - m_limelightTable.getEntry("snapshot").setNumber(0); // Reset - } - - /** - * Set the camera's pose in the coordinate system of the robot. - * @param pose - */ - public void setCameraPose(Pose3d pose) { - var poseData = new double[] { - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getTranslation().getZ(), - Units.radiansToDegrees(pose.getRotation().getX()), - Units.radiansToDegrees(pose.getRotation().getY()), + m_limelightTable.getEntry("ledMode").setNumber(1); + try { + Thread.sleep(200); + } catch (Exception e) { + Thread.currentThread().interrupt(); + } + } + + // Then return to pipeline control + setLedMode(0); + }); + } + + /** + * Sets limelight’s operation mode. 0 = Vision processor. 1 = Driver Camera (Increases exposure, disables vision + * processing). + * + * @param mode + */ + public void setCameraMode(int mode) { + m_limelightTable.getEntry("camMode").setNumber(mode); + } + + /** + * Sets limelight’s pipeline. + * + * @param pipeline + */ + public void setPipeline(int pipeline) { + m_limelightTable.getEntry("pipeline").setNumber(pipeline); + } + + /** + * Side-by-side streams (Note: USB output stream is not affected by this mode) + * + * @param mode + */ + public void setPiPStreamingMode(int mode) { + m_limelightTable.getEntry("stream").setNumber(mode); + } + + /** + * Allows users to take snapshots during a match + */ + public void takeSnapshot() { + m_limelightTable.getEntry("snapshot").setNumber(0); // Reset + m_limelightTable.getEntry("snapshot").setNumber(1); // Take snapshot + m_limelightTable.getEntry("snapshot").setNumber(0); // Reset + } + + /** + * Set the camera's pose in the coordinate system of the robot. + * + * @param pose + */ + public void setCameraPose(Pose3d pose) { + var poseData = new double[] { + pose.getTranslation().getX(), pose.getTranslation().getY(), pose.getTranslation().getZ(), + Units.radiansToDegrees(pose.getRotation().getX()), Units.radiansToDegrees(pose.getRotation().getY()), Units.radiansToDegrees(pose.getRotation().getZ()), - }; + }; - m_limelightTable.getEntry("camerapose_robotspace_set").setDoubleArray(poseData); - } + m_limelightTable.getEntry("camerapose_robotspace_set").setDoubleArray(poseData); + } - //#endregion + // #endregion - public void close() { - m_executorService.shutdown(); - } + public void close() { + m_executorService.shutdown(); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index cc94f3d..6c13aaf 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -13,149 +13,150 @@ @Logged(strategy = Strategy.OPT_IN) public class VisionSubsystem extends SubsystemBase { - private LimeLightNT[] m_limelights; - - private LimelightInputs[] m_limelightInputs; - private LimelightPose[] m_limelightRobotPoses; - @Logged(name = "ArrayElementCount", importance = Logged.Importance.CRITICAL) - private int arrayElementCount; - - public VisionSubsystem() { - setName("VisionSubsystem"); - var defaultInstance = NetworkTableInstance.getDefault(); - - m_limelights = new LimeLightNT[] { - new LimeLightNT(defaultInstance, DriveMap.LimelightFrontName), - new LimeLightNT(defaultInstance, DriveMap.LimelightRearName) - }; - m_limelightInputs = new LimelightInputs[] { - new LimelightInputs(), - new LimelightInputs() - }; - m_limelightRobotPoses = new LimelightPose[] { - m_limelightInputs[0].FieldSpaceRobotPose, - m_limelightInputs[1].FieldSpaceRobotPose - }; - arrayElementCount = m_limelightInputs.length; - } - - /** - * Gets the inputs for the specified limelight. - * @param llIndex The index of the limelight to get inputs from. - */ - public LimelightInputs getLimelightInputs(int llIndex) { - return m_limelightInputs[llIndex]; - } - - /** - * Gets all limelight inputs - */ - @Logged(name = "LimelightInputs", importance = Logged.Importance.CRITICAL) - public LimelightInputs[] getAllLimelightInputs() { - return m_limelightInputs; - } - - /** - * Gets all limelight inputs - */ - @Logged(name = "LimelightPoses", importance = Logged.Importance.CRITICAL) - public LimelightPose[] getAllFieldRobotPoses() { - return m_limelightRobotPoses; - } - - /** - * Sets limelight’s LED state. - * 0 = use the LED Mode set in the current pipeline. - * 1 = force off. - * 2 = force blink. - * 3 = force on. - * @param llIndex The index of the desired limelight - * @param mode The LED mode to set - */ - public void setLedMode(int llIndex, int mode) { - m_limelights[llIndex].setLedMode(mode); - } - - /** - * Forces the LED to blink a specified number of times, then returns to pipeline control. - * @param llIndex The index of the desired limelight - * @param blinkCount The number of times to blink the LED - */ - public void blinkLed(int llIndex, int blinkCount) { - m_limelights[llIndex].blinkLed(blinkCount); - } - - /** - * Sets limelight’s operation mode. - * 0 = Vision processor. - * 1 = Driver Camera (Increases exposure, disables vision processing). - * @param llIndex The index of the desired limelight - * @param mode The camera mode to set - */ - public void setCameraMode(int llIndex, int mode) { - m_limelights[llIndex].setCameraMode(mode); - } - - /** - * Sets limelight’s active vision pipeline. - * @param llIndex The index of the desired limelight - * @param pipeline The pipeline to set active - */ - public void setPipeline(int llIndex, int pipeline) { - m_limelights[llIndex].setPipeline(pipeline); - } - - /** - * Sets limelight’s streaming mode. - * 0 = Standard - Side-by-side streams if a webcam is attached to Limelight - * 1 = PiP Main - The secondary camera stream is placed in the lower-right corner of the primary camera stream - * 2 = PiP Secondary - The primary camera stream is placed in the lower-right corner of the secondary camera stream - * @param llIndex The index of the desired limelight - * @param mode The streaming mode to set - */ - public void setPiPStreamingMode(int llIndex, int mode) { - m_limelights[llIndex].setPiPStreamingMode(mode); - } - - /** - * Takes an instantaneous snapshot of the limelight's camera feed. - * @param llIndex The index of the desired limelight - */ - public void takeSnapshot(int llIndex) { - m_limelights[llIndex].takeSnapshot(); - } - - /** - * Set the camera's pose in the coordinate system of the robot. - * @param llIndex The index of the desired limelight - * @param pose The Camera's pose to set in Robot space - */ - public void setCameraPose(int llIndex, Pose3d pose) { - m_limelights[llIndex].setCameraPose(pose); - } - - public void periodic() { - // Update all limelight inputs - // for (int i = 0; i < m_limelights.length; i++) { - // m_limelightInputs[i] = m_limelights[i].getInputs(); - // } - arrayElementCount = m_limelightInputs.length; - - // Update Dashboard & logging - var frontInputs = getLimelightInputs(0); - var rearInputs = getLimelightInputs(1); - SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", isAprilTagIdValid(frontInputs.ApriltagId)); - SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isAprilTagIdValid(rearInputs.ApriltagId)); - DriverDashboard.FrontApTagIdField.setDouble(frontInputs.ApriltagId); - DriverDashboard.RearApTagIdField.setDouble(rearInputs.ApriltagId); - DriverDashboard.RearApTagOffsetDial.setDouble(rearInputs.TargetHorizontalOffset.getDegrees()); - } - - public static boolean isAprilTagIdASpeakerCenterTarget(int apriltagId) { - return apriltagId == 4 || apriltagId == 7; - } - - public static boolean isAprilTagIdValid(int apriltagId) { - return apriltagId >= 1 && apriltagId <= 16; - } + private LimeLightNT[] m_limelights; + + private LimelightInputs[] m_limelightInputs; + private LimelightPose[] m_limelightRobotPoses; + @Logged(name = "ArrayElementCount", importance = Logged.Importance.CRITICAL) + private int arrayElementCount; + + public VisionSubsystem() { + setName("VisionSubsystem"); + var defaultInstance = NetworkTableInstance.getDefault(); + + m_limelights = new LimeLightNT[] { + new LimeLightNT(defaultInstance, DriveMap.LimelightFrontName), + new LimeLightNT(defaultInstance, DriveMap.LimelightRearName) + }; + m_limelightInputs = new LimelightInputs[] { + new LimelightInputs(), new LimelightInputs() + }; + m_limelightRobotPoses = new LimelightPose[] { + m_limelightInputs[0].FieldSpaceRobotPose, m_limelightInputs[1].FieldSpaceRobotPose + }; + arrayElementCount = m_limelightInputs.length; + } + + /** + * Gets the inputs for the specified limelight. + * + * @param llIndex The index of the limelight to get inputs from. + */ + public LimelightInputs getLimelightInputs(int llIndex) { + return m_limelightInputs[llIndex]; + } + + /** + * Gets all limelight inputs + */ + @Logged(name = "LimelightInputs", importance = Logged.Importance.CRITICAL) + public LimelightInputs[] getAllLimelightInputs() { + return m_limelightInputs; + } + + /** + * Gets all limelight inputs + */ + @Logged(name = "LimelightPoses", importance = Logged.Importance.CRITICAL) + public LimelightPose[] getAllFieldRobotPoses() { + return m_limelightRobotPoses; + } + + /** + * Sets limelight’s LED state. 0 = use the LED Mode set in the current pipeline. 1 = force off. 2 = force blink. 3 = + * force on. + * + * @param llIndex The index of the desired limelight + * @param mode The LED mode to set + */ + public void setLedMode(int llIndex, int mode) { + m_limelights[llIndex].setLedMode(mode); + } + + /** + * Forces the LED to blink a specified number of times, then returns to pipeline control. + * + * @param llIndex The index of the desired limelight + * @param blinkCount The number of times to blink the LED + */ + public void blinkLed(int llIndex, int blinkCount) { + m_limelights[llIndex].blinkLed(blinkCount); + } + + /** + * Sets limelight’s operation mode. 0 = Vision processor. 1 = Driver Camera (Increases exposure, disables vision + * processing). + * + * @param llIndex The index of the desired limelight + * @param mode The camera mode to set + */ + public void setCameraMode(int llIndex, int mode) { + m_limelights[llIndex].setCameraMode(mode); + } + + /** + * Sets limelight’s active vision pipeline. + * + * @param llIndex The index of the desired limelight + * @param pipeline The pipeline to set active + */ + public void setPipeline(int llIndex, int pipeline) { + m_limelights[llIndex].setPipeline(pipeline); + } + + /** + * Sets limelight’s streaming mode. 0 = Standard - Side-by-side streams if a webcam is attached to Limelight 1 = PiP + * Main - The secondary camera stream is placed in the lower-right corner of the primary camera stream 2 = PiP + * Secondary - The primary camera stream is placed in the lower-right corner of the secondary camera stream + * + * @param llIndex The index of the desired limelight + * @param mode The streaming mode to set + */ + public void setPiPStreamingMode(int llIndex, int mode) { + m_limelights[llIndex].setPiPStreamingMode(mode); + } + + /** + * Takes an instantaneous snapshot of the limelight's camera feed. + * + * @param llIndex The index of the desired limelight + */ + public void takeSnapshot(int llIndex) { + m_limelights[llIndex].takeSnapshot(); + } + + /** + * Set the camera's pose in the coordinate system of the robot. + * + * @param llIndex The index of the desired limelight + * @param pose The Camera's pose to set in Robot space + */ + public void setCameraPose(int llIndex, Pose3d pose) { + m_limelights[llIndex].setCameraPose(pose); + } + + public void periodic() { + // Update all limelight inputs + // for (int i = 0; i < m_limelights.length; i++) { + // m_limelightInputs[i] = m_limelights[i].getInputs(); + // } + arrayElementCount = m_limelightInputs.length; + + // Update Dashboard & logging + var frontInputs = getLimelightInputs(0); + var rearInputs = getLimelightInputs(1); + SmartDashboard.putBoolean("Drive/PoseEstimation/Front/IsValidTarget", isAprilTagIdValid(frontInputs.ApriltagId)); + SmartDashboard.putBoolean("Drive/PoseEstimation/Rear/IsValidTarget", isAprilTagIdValid(rearInputs.ApriltagId)); + DriverDashboard.FrontApTagIdField.setDouble(frontInputs.ApriltagId); + DriverDashboard.RearApTagIdField.setDouble(rearInputs.ApriltagId); + DriverDashboard.RearApTagOffsetDial.setDouble(rearInputs.TargetHorizontalOffset.getDegrees()); + } + + public static boolean isAprilTagIdASpeakerCenterTarget(int apriltagId) { + return apriltagId == 4 || apriltagId == 7; + } + + public static boolean isAprilTagIdValid(int apriltagId) { + return apriltagId >= 1 && apriltagId <= 16; + } } From 7cf4e1d9a8a9f7e77c755679b3bf1d1a836f3572 Mon Sep 17 00:00:00 2001 From: Mason H <35619771+zeroClearAmerican@users.noreply.github.com> Date: Mon, 18 Nov 2024 22:36:06 -0600 Subject: [PATCH 21/24] revised deprecated feedforward function --- .../subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java index baa92d6..0f30aea 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.java @@ -79,7 +79,7 @@ private void setDesiredState(SwerveModuleState desiredState) { // Set the drive motor to the desired speed // Calculate target data to voltage data var velocityRadPerSec = desiredState.speedMetersPerSecond / (DriveMap.DriveWheelDiameterMeters / 2); - var driveAppliedVolts = m_driveFeedforward.calculate(velocityRadPerSec) + var driveAppliedVolts = m_driveFeedforward.calculate(Units.RadiansPerSecond.of(velocityRadPerSec)).magnitude() + m_driveFeedback.calculate(m_driveMotorSim.getAngularVelocityRadPerSec(), velocityRadPerSec); driveAppliedVolts = MathUtil.clamp(driveAppliedVolts, -12.0, 12.0); From 70a715cf9d6c5839c9ae42e4284984acb8614f2e Mon Sep 17 00:00:00 2001 From: matthew mcconaughey <35619771+zeroClearAmerican@users.noreply.github.com> Date: Mon, 25 Nov 2024 16:30:21 -0600 Subject: [PATCH 22/24] Merge ShooterSubsystem IOC impl (#5) * Started implementing shooter interface (incomplete) * Finished implementing ShooterIOReal * General clean up and minor changes to the Shooter Subsytem. Also changed Shooter Subsytem creation in Container to pass through "isReal" (still commented out) * Made Shooter Inputs and Outputs more readable and did more general cleanup * Fixed misspelling "DriveIO" -> "ShooterIO" * Condensed and simplified the setting of the inputs and outputs for the elevationSolenoid * Added the getting of inputs and setting of outputs to ShooterSubsystem's periodic * Made it so TalonSpeed and VictorSpeed are set to 0 in ShooterSubsystem.stopMotors() rather than ShooterIOReal.StopMotors() and made the motors only get set to a new value when needed (when the desired value differs from the current value) * Removed unnecessary @parm from IShooterIO.StopMotors() JavaDoc * Made it so m_previousTalonSpeed and m_previousVictorSpeed are set to 0 in ShooterIOReal.StopMotors() to prevent the motors from being set at the same time StopMotors() is being called * Fixed logging in IShooterIO and uncommented the creation of ShooterSubsystem from Container * Uncommented lines from Robot relating to ShooterSubsystem (in autonomousInit and teleopInit) --------- Co-authored-by: ArrowVark Co-authored-by: Hudson <102180082+ArrowVark@users.noreply.github.com> --- src/main/java/frc/robot/CombinedCommands.java | 10 +- src/main/java/frc/robot/Container.java | 8 +- src/main/java/frc/robot/Robot.java | 4 +- .../robot/subsystems/shooter/IShooterIO.java | 28 ++++ .../subsystems/shooter/ShooterIOInputs.java | 20 +++ .../subsystems/shooter/ShooterIOOutputs.java | 17 +++ .../subsystems/shooter/ShooterIOReal.java | 113 ++++++++++++++++ .../subsystems/shooter/ShooterIOSim.java | 24 ++++ .../ShooterSubsystem.java} | 125 ++++++++++-------- 9 files changed, 281 insertions(+), 68 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/IShooterIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOInputs.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOOutputs.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java rename src/main/java/frc/robot/subsystems/{Shooter.java => shooter/ShooterSubsystem.java} (62%) diff --git a/src/main/java/frc/robot/CombinedCommands.java b/src/main/java/frc/robot/CombinedCommands.java index 49381e5..b901e17 100644 --- a/src/main/java/frc/robot/CombinedCommands.java +++ b/src/main/java/frc/robot/CombinedCommands.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.shooter.ShooterSubsystem; public class CombinedCommands { /** @@ -16,7 +16,7 @@ public class CombinedCommands { * * @return */ - public SequentialCommandGroup scoreInSpeakerSequentialGroup(Shooter shooter, Intake intake) { + public SequentialCommandGroup scoreInSpeakerSequentialGroup(ShooterSubsystem shooter, Intake intake) { return shooter.startShootingNoteCommand().andThen(new WaitCommand(0.75)).andThen(intake.ejectNoteCommand()) .andThen(new WaitCommand(0.75)).andThen(shooter.stopMotorsCommand()).andThen(intake.stopRollersCommand()); } @@ -26,7 +26,7 @@ public SequentialCommandGroup scoreInSpeakerSequentialGroup(Shooter shooter, Int * * @return */ - public SequentialCommandGroup loadNoteForAmp(Shooter shooter, Intake intake) { + public SequentialCommandGroup loadNoteForAmp(ShooterSubsystem shooter, Intake intake) { return Commands.runOnce(() -> intake.runIntakeRollers(-0.7)) // Eject from the intake .alongWith(Commands.runOnce(() -> shooter.runShooter(0.1))) // Load into the shooter .andThen(new WaitUntilCommand(shooter::isNoteLoaded).withTimeout(1)) // Wait until the note is loaded @@ -39,7 +39,7 @@ public SequentialCommandGroup loadNoteForAmp(Shooter shooter, Intake intake) { * * @return */ - public SequentialCommandGroup stopShooterAndIntakeCommand(Shooter shooter, Intake intake) { + public SequentialCommandGroup stopShooterAndIntakeCommand(ShooterSubsystem shooter, Intake intake) { return shooter.stopMotorsCommand().andThen(intake.stopRollersCommand()); } @@ -48,7 +48,7 @@ public SequentialCommandGroup stopShooterAndIntakeCommand(Shooter shooter, Intak * * @return */ - public Map getNamedCommands(Shooter shooter, Intake intake) { + public Map getNamedCommands(ShooterSubsystem shooter, Intake intake) { return Map.of("Score_In_Speaker", scoreInSpeakerSequentialGroup(shooter, intake), "Load_Note_For_Amp", loadNoteForAmp(shooter, intake), "Stop_Shooter_And_Intake", stopShooterAndIntakeCommand(shooter, intake)); } diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index 3fee3d3..d847ac9 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.*; import frc.robot.maps.DriveMap; import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; import prime.control.Controls; @@ -34,7 +35,8 @@ public class Container { public VisionSubsystem Vision; @Logged(name = "Drive", importance = Importance.CRITICAL) public DrivetrainSubsystem Drivetrain; - // public Shooter Shooter; + @Logged(name = "Shooter", importance = Importance.CRITICAL) + public ShooterSubsystem Shooter; // public Intake Intake; // public Climbers Climbers; // public PwmLEDs LEDs; @@ -55,9 +57,7 @@ public Container(boolean isReal) { Vision = new VisionSubsystem(); Drivetrain = new DrivetrainSubsystem(isReal, LEDs::clearForegroundPattern, LEDs::setForegroundPattern, Vision::getAllLimelightInputs); - // Shooter = new Shooter( - // LEDs::clearForegroundPattern, - // LEDs::setForegroundPattern); + Shooter = new ShooterSubsystem(LEDs::clearForegroundPattern, LEDs::setForegroundPattern); // Intake = new Intake(); // Climbers = new Climbers(); // Compressor = new Compressor(30, PneumaticsModuleType.REVPH); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d181ee6..264b1ee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -99,7 +99,7 @@ public void autonomousInit() { m_autonomousCommand.cancel(); // Stop the shooter and intake motors in case they're still running and set the intake IN - // m_robotContainer.Shooter.stopMotorsCommand().schedule(); + m_robotContainer.Shooter.stopMotorsCommand().schedule(); // m_robotContainer.Intake.stopRollersCommand().schedule(); // m_robotContainer.Intake.setIntakeInCommand().schedule(); } @@ -134,7 +134,7 @@ public void teleopInit() { m_autonomousCommand.cancel(); // Stop the shooter and intake motors in case they're still running - // m_robotContainer.Shooter.stopMotorsCommand().schedule(); + m_robotContainer.Shooter.stopMotorsCommand().schedule(); // m_robotContainer.Intake.stopRollersCommand().schedule(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/IShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/IShooterIO.java new file mode 100644 index 0000000..5b6f10e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/IShooterIO.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; + +public interface IShooterIO { + + /** + * Gets the inputs from the robot to be used in the code + * @return ShooterIOInputs + */ + public ShooterIOInputs getInputs(); + + /** + * Sets the outputs given by the code to be used in the robot + * @param outputs + */ + public void setOutputs(ShooterIOOutputs outputs); + + /** + * Directly interacts with IShooterIO, stopping the motors in the shooter + * @implNote Version declared in the Shooter IO. + * This method is used to directly communicate to the interface and whichever class is currently running (real or sim). + * This should mainly only be called from within methods in the ShooterSubsytem class. + * @see ShooterSubsystem stopMotors() + */ + public void StopMotors(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOInputs.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOInputs.java new file mode 100644 index 0000000..551082f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOInputs.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.epilogue.Logged; + + +/** + * The inputs used by the shooter subsystem + */ +@Logged +public class ShooterIOInputs { + + public double TalonState; + public double TalonVelocity; + public double VictorOutput; + + public boolean ElevationSolenoidState; + + public boolean NoteDetectorState; + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOOutputs.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOOutputs.java new file mode 100644 index 0000000..867b5e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOOutputs.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.epilogue.Logged; + + +/** + * The outputs used by the shooter subsytem + */ +@Logged +public class ShooterIOOutputs { + + public double VictorSpeed; + public double TalonSpeed; + + public boolean ElevationSolenoidValue; + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java new file mode 100644 index 0000000..5c261e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -0,0 +1,113 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +@Logged(strategy = Strategy.OPT_IN) +public class ShooterIOReal implements IShooterIO { + + private TalonFX m_talonFX; + private VictorSPX m_victorSPX; + private DoubleSolenoid m_elevationSolenoid; + private DigitalInput m_noteDetector; + + private double m_previousTalonSpeed = 0; + private double m_previousVictorSpeed = 0; + + public ShooterIOReal() { + + m_talonFX = new TalonFX(ShooterSubsystem.VMap.TalonFXCanID); + var talonConfig = new TalonFXConfiguration(); + talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + m_talonFX.getConfigurator().apply(talonConfig); + m_talonFX.setNeutralMode(NeutralModeValue.Brake); + + m_victorSPX = new VictorSPX(ShooterSubsystem.VMap.VictorSPXCanID); + m_victorSPX.configFactoryDefault(); + m_victorSPX.setNeutralMode(NeutralMode.Brake); + + m_elevationSolenoid = + new DoubleSolenoid( + 30, + PneumaticsModuleType.REVPH, + ShooterSubsystem.VMap.ElevationSolenoidForwardChannel, + ShooterSubsystem.VMap.ElevationSolenoidReverseChannel + ); + + m_noteDetector = new DigitalInput(ShooterSubsystem.VMap.NoteDetectorDIOChannel); + } + + //#region IO Methods + + @Override + public ShooterIOInputs getInputs() { + var inputs = new ShooterIOInputs(); + + inputs.TalonState = m_talonFX.get(); + inputs.TalonVelocity = m_talonFX.getVelocity().getValueAsDouble(); + + inputs.VictorOutput = m_victorSPX.getMotorOutputPercent(); + + inputs.ElevationSolenoidState = ( + m_elevationSolenoid.get() == Value.kForward + ? true + : false + ); + + inputs.NoteDetectorState = !m_noteDetector.get(); + + return inputs; + } + + @Override + public void setOutputs(ShooterIOOutputs outputs) { + + if (outputs.TalonSpeed != m_previousTalonSpeed) { + + m_talonFX.set(outputs.TalonSpeed); + + m_previousTalonSpeed = outputs.TalonSpeed; + } + + if (outputs.VictorSpeed != m_previousVictorSpeed) { + + m_victorSPX.set(VictorSPXControlMode.PercentOutput, outputs.VictorSpeed); + + m_previousVictorSpeed = outputs.VictorSpeed; + } + + + m_elevationSolenoid.set( + outputs.ElevationSolenoidValue + ? Value.kForward + : Value.kReverse + ); + } + + @Override + public void StopMotors() { + + m_previousTalonSpeed = 0; + m_previousVictorSpeed = 0; + + m_talonFX.set(0); + m_talonFX.stopMotor(); + + m_victorSPX.set(VictorSPXControlMode.PercentOutput, 0); + + } + + //#endregion +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java new file mode 100644 index 0000000..cae7eee --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; + +@Logged(strategy = Strategy.OPT_IN) +public class ShooterIOSim implements IShooterIO { + + @Override + public ShooterIOInputs getInputs() { + return null; + } + + @Override + public void setOutputs(ShooterIOOutputs outputs) { + + } + + @Override + public void StopMotors() { + + } + +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java similarity index 62% rename from src/main/java/frc/robot/subsystems/Shooter.java rename to src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index ed9f3d4..c8af9c4 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -1,29 +1,22 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; +package frc.robot.subsystems.shooter; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import java.util.Map; import java.util.function.Consumer; -public class Shooter extends SubsystemBase { +@Logged(strategy = Strategy.OPT_IN) +public class ShooterSubsystem extends SubsystemBase { public static class VMap { @@ -36,11 +29,6 @@ public static class VMap { public static final int ElevationSolenoidReverseChannel = 7; } - private TalonFX m_talonFX; - private VictorSPX m_victorSPX; - private DoubleSolenoid m_elevationSolenoid; - private DigitalInput m_noteDetector; - private Runnable m_clearForegroundPatternFunc; private Consumer m_setForegroundPatternFunc; private LEDPattern m_elevatorUpLEDPattern = LEDPattern.solid(Color.kWhite); @@ -49,38 +37,31 @@ public static class VMap { private LEDPattern m_noteDetectedLEDPattern = LEDPattern.solid(Color.kOrange) .blink(Units.Seconds.of(0.2)); + private IShooterIO m_shooterio = ( + Robot.isReal() + ? new ShooterIOReal() + : new ShooterIOSim() + ); + @Logged(name = "ShooterIOInputs", importance = Logged.Importance.CRITICAL) + private ShooterIOInputs m_inputs; + @Logged(name = "ShooterIOOutputs", importance = Logged.Importance.CRITICAL) + private ShooterIOOutputs m_outputs; + // #endregion /** * Creates a new Shooter with a given configuration * @param config */ - public Shooter( + public ShooterSubsystem( Runnable restoreLEDPersistentPatternFunc, Consumer setLEDTemporaryPatternFunc ) { setName("Shooter"); - m_talonFX = new TalonFX(VMap.TalonFXCanID); - var talonConfig = new TalonFXConfiguration(); - talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - m_talonFX.getConfigurator().apply(talonConfig); - m_talonFX.setNeutralMode(NeutralModeValue.Brake); - - m_victorSPX = new VictorSPX(VMap.VictorSPXCanID); - m_victorSPX.configFactoryDefault(); - m_victorSPX.setNeutralMode(NeutralMode.Brake); - - m_elevationSolenoid = - new DoubleSolenoid( - 30, - PneumaticsModuleType.REVPH, - VMap.ElevationSolenoidForwardChannel, - VMap.ElevationSolenoidReverseChannel - ); - - m_noteDetector = new DigitalInput(VMap.NoteDetectorDIOChannel); - + m_inputs = m_shooterio.getInputs(); + m_outputs = new ShooterIOOutputs(); + m_clearForegroundPatternFunc = restoreLEDPersistentPatternFunc; m_setForegroundPatternFunc = setLEDTemporaryPatternFunc; } @@ -92,33 +73,50 @@ public Shooter( * @param speed */ public void runShooter(double speed) { - m_talonFX.set(speed); - m_victorSPX.set(VictorSPXControlMode.PercentOutput, speed * 3); + m_outputs.TalonSpeed = speed; + m_outputs.VictorSpeed = speed * 3; } public void runGreenWheel(double speed) { - m_victorSPX.set(VictorSPXControlMode.PercentOutput, speed); + m_outputs.VictorSpeed = speed; } /** * Stops the shooter motors + * @implNote Version declared in the ShooterSubsystem. + * This is the method that, unlike StopMotors(), interacts with the lights on the robot. Because of this, this method will be used almost everywhere. + * ( StopMotors() in IShooterIO behaves more like multiple lines of code stopping the motors, while stopMotors() in ShooterSubsystem ties it all together. ) + * @see IShooterIO StopMotors() */ public void stopMotors() { - m_talonFX.stopMotor(); - m_victorSPX.set(VictorSPXControlMode.PercentOutput, 0); + m_shooterio.StopMotors(); + + m_outputs.TalonSpeed = 0; + m_outputs.VictorSpeed = 0; + m_clearForegroundPatternFunc.run(); } /** * Gets a boolean indicating whether a note is blocking the beam sensor - * @return + * @return boolean */ public boolean isNoteLoaded() { - return !m_noteDetector.get(); + return m_inputs.NoteDetectorState; } + /** + * Sets the elevator to either kForward or kReverse ( kOff not supported and will instead act as if it is kReverse ). + * Works by checking if value is kForward, then setting ElevationSolenoidValue to true if they are equal and false if they are not. + * @implNote A positive ElevationSolenoidValue corresponds to kForward and vice versa + * @param value + */ public void setElevator(Value value) { - m_elevationSolenoid.set(value); + m_outputs.ElevationSolenoidValue = ( + value == Value.kForward + ? true + : false + ); } public void setElevatorUp() { @@ -137,6 +135,9 @@ public void setElevatorDown() { @Override public void periodic() { + // Get inputs + m_inputs = m_shooterio.getInputs(); + var newNoteDetectedValue = isNoteLoaded(); if (newNoteDetectedValue != m_lastNoteDetectedValue) { if (newNoteDetectedValue && !m_lastNoteDetectedValue) { @@ -147,12 +148,15 @@ public void periodic() { // Save the new value m_lastNoteDetectedValue = newNoteDetectedValue; + + // Send outputs to the shooter IO + m_shooterio.setOutputs(m_outputs); } // Level2 Logging - SmartDashboard.putNumber("Shooter/LaunchMotorOutput", m_talonFX.get()); - SmartDashboard.putNumber("Shooter/LaunchMotorVelocity", m_talonFX.getVelocity().getValueAsDouble()); - SmartDashboard.putNumber("Shooter/GuideMotorOutput", m_victorSPX.getMotorOutputPercent()); + SmartDashboard.putNumber("Shooter/LaunchMotorOutput", m_inputs.TalonState); + SmartDashboard.putNumber("Shooter/LaunchMotorVelocity", m_inputs.TalonVelocity); + SmartDashboard.putNumber("Shooter/GuideMotorOutput", m_inputs.VictorOutput); SmartDashboard.putBoolean("Shooter/NoteDetected", newNoteDetectedValue); } @@ -160,7 +164,7 @@ public void periodic() { /** * Stops the shooter motors - * @return + * @return Command */ public Command stopMotorsCommand() { return Commands.runOnce(() -> stopMotors()); @@ -168,7 +172,7 @@ public Command stopMotorsCommand() { /** * Shootes a note at half speed - * @return + * @return Command */ public Command scoreInAmpCommand() { return Commands.run(() -> runShooter(0.5)); @@ -176,7 +180,7 @@ public Command scoreInAmpCommand() { /** * Shootes a note at full speed - * @return + * @return Command */ public Command startShootingNoteCommand() { return Commands.runOnce(() -> { @@ -187,7 +191,7 @@ public Command startShootingNoteCommand() { /** * Sets the elevation of the shooter all the way up - * @return + * @return Command */ public Command setElevationUpCommand() { return Commands.runOnce(this::setElevatorUp); @@ -195,7 +199,7 @@ public Command setElevationUpCommand() { /** * Sets the elevation of the shooter all the way down - * @return + * @return Command */ public Command setElevationDownCommand() { return Commands.runOnce(this::setElevatorDown); @@ -203,14 +207,21 @@ public Command setElevationDownCommand() { /** * Toggles the elevation of the shooter up/down - * @return + * @return Command */ public Command toggleElevationCommand() { return Commands.runOnce(() -> { - if (m_elevationSolenoid.get() == Value.kForward) setElevatorDown(); else setElevatorUp(); + if (m_inputs.ElevationSolenoidState == true) { + setElevatorDown(); + } else { + setElevatorUp(); + } }); } + /** + * Links named commands used in PathPlanner to methods in the subsystem + */ public Map getNamedCommands() { return Map.of( "Set_Elevation_Up", From 50f0391cc0aebb0144573dee1b27cf44abbea166 Mon Sep 17 00:00:00 2001 From: Arts <140666439+ArtsNCrafters@users.noreply.github.com> Date: Tue, 17 Dec 2024 16:57:47 -0600 Subject: [PATCH 23/24] climb IO to drivetain branch request (#6) * currently impletmint IO * started working on IO, finished IOReal * Renamed Climbers to ClimbersSubsystem, added @logged * Finised converting climbers to IO, deleted merge conflicts --------- Co-authored-by: Zachary --- .../frc/robot/subsystems/ClimbersIOReal.java | 5 + .../subsystems/climbers/ClimbersIOInputs.java | 9 + .../climbers/ClimbersIOOutputs.java | 13 ++ .../subsystems/climbers/ClimbersIOReal.java | 74 ++++++++ .../subsystems/climbers/ClimbersIOSim.java | 22 +++ .../ClimbersSubsystem.java} | 179 ++++++++---------- .../subsystems/climbers/IClimbersIO.java | 14 ++ 7 files changed, 217 insertions(+), 99 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ClimbersIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/climbers/ClimbersIOInputs.java create mode 100644 src/main/java/frc/robot/subsystems/climbers/ClimbersIOOutputs.java create mode 100644 src/main/java/frc/robot/subsystems/climbers/ClimbersIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/climbers/ClimbersIOSim.java rename src/main/java/frc/robot/subsystems/{Climbers.java => climbers/ClimbersSubsystem.java} (53%) create mode 100644 src/main/java/frc/robot/subsystems/climbers/IClimbersIO.java diff --git a/src/main/java/frc/robot/subsystems/ClimbersIOReal.java b/src/main/java/frc/robot/subsystems/ClimbersIOReal.java new file mode 100644 index 0000000..3d25488 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimbersIOReal.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems; + +public class ClimbersIOReal { + +} diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimbersIOInputs.java b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOInputs.java new file mode 100644 index 0000000..ce13cd9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOInputs.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.climbers; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj.DigitalInput; +@Logged +public class ClimbersIOInputs { + public Boolean leftLimitSwitch; + public Boolean rightLimitSwitch; +} diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimbersIOOutputs.java b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOOutputs.java new file mode 100644 index 0000000..54f8e01 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOOutputs.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.climbers; + +import edu.wpi.first.epilogue.Logged; + +@Logged +public class ClimbersIOOutputs { + + public double leftMotorOutput; + public double rightMotorOutput; + + public boolean ClutchLeft; + public boolean ClutchRight; +} diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimbersIOReal.java b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOReal.java new file mode 100644 index 0000000..53002f3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOReal.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.climbers; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import frc.robot.subsystems.climbers.ClimbersSubsystem.Side; + +@Logged(strategy = Strategy.OPT_IN) +public class ClimbersIOReal implements IClimbersIO{ + public DigitalInput m_leftLimitSwitch; + public DigitalInput m_rightLimitSwitch; + + public VictorSPX m_leftVictorSPX; + public VictorSPX m_rightVictorSPX; + + public DoubleSolenoid m_clutchSolenoidLeft; + public DoubleSolenoid m_clutchSolenoidRight; + + public ClimbersIOReal(){ + m_leftVictorSPX = new VictorSPX(ClimbersSubsystem.VMap.VictorSPXLeftCanID); + m_leftVictorSPX.configFactoryDefault(); + m_leftVictorSPX.setInverted(ClimbersSubsystem.VMap.LeftInverted); + m_leftVictorSPX.setNeutralMode(NeutralMode.Brake); + m_leftVictorSPX.configOpenloopRamp(0.5); + + m_rightVictorSPX = new VictorSPX(ClimbersSubsystem.VMap.VictorSPXRightCanID); + m_rightVictorSPX.configFactoryDefault(); + m_rightVictorSPX.setInverted(ClimbersSubsystem.VMap.RightInverted); + m_rightVictorSPX.setNeutralMode(NeutralMode.Brake); + m_rightVictorSPX.configOpenloopRamp(0.5); + + m_leftLimitSwitch = new DigitalInput(ClimbersSubsystem.VMap.LeftLimitSwitchDIOChannel); + m_rightLimitSwitch = new DigitalInput(ClimbersSubsystem.VMap.RightLimitSwitchDIOChannel); + + m_clutchSolenoidLeft = + new DoubleSolenoid( + 30, + PneumaticsModuleType.REVPH, + ClimbersSubsystem.VMap.LeftSolenoidForwardChannel, + ClimbersSubsystem.VMap.LeftSolenoidReverseChannel + ); + m_clutchSolenoidRight = + new DoubleSolenoid( + 30, + PneumaticsModuleType.REVPH, + ClimbersSubsystem.VMap.RightSolenoidForwardChannel, + ClimbersSubsystem.VMap.RightSolenoidReverseChannel + ); + } + + public ClimbersIOInputs getInputs() { + var inputs = new ClimbersIOInputs(); + inputs.leftLimitSwitch = m_leftLimitSwitch.get(); + inputs.rightLimitSwitch = m_rightLimitSwitch.get(); + return inputs; + + } + + public void setOutputs(ClimbersIOOutputs outputs){ + m_leftVictorSPX.set(VictorSPXControlMode.PercentOutput, outputs.leftMotorOutput); + m_rightVictorSPX.set(VictorSPXControlMode.PercentOutput, outputs.rightMotorOutput); + m_clutchSolenoidLeft.set(outputs.ClutchLeft ? Value.kForward : Value.kReverse); + m_clutchSolenoidRight.set(outputs.ClutchRight ? Value.kForward : Value.kReverse); + } + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climbers/ClimbersIOSim.java b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOSim.java new file mode 100644 index 0000000..e8f0949 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climbers/ClimbersIOSim.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.climbers; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.wpilibj.DigitalInput; + +@Logged(strategy = Strategy.OPT_IN) +public class ClimbersIOSim implements IClimbersIO{ + public DigitalInput leftLimitSwitch; + public DigitalInput rightLimitSwitch; + + @Override + public ClimbersIOInputs getInputs() { + return null; + } + + @Override + public void setOutputs(ClimbersIOOutputs outputs) { + + } + +} diff --git a/src/main/java/frc/robot/subsystems/Climbers.java b/src/main/java/frc/robot/subsystems/climbers/ClimbersSubsystem.java similarity index 53% rename from src/main/java/frc/robot/subsystems/Climbers.java rename to src/main/java/frc/robot/subsystems/climbers/ClimbersSubsystem.java index 85f83ee..1536485 100644 --- a/src/main/java/frc/robot/subsystems/Climbers.java +++ b/src/main/java/frc/robot/subsystems/climbers/ClimbersSubsystem.java @@ -1,9 +1,11 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.climbers; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -20,7 +22,8 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -public class Climbers extends SubsystemBase { +@Logged(strategy = Strategy.OPT_IN) +public class ClimbersSubsystem extends SubsystemBase { public static class VMap { @@ -43,17 +46,9 @@ public enum Side { kRight, } - // Motors - private VictorSPX m_leftVictorSPX; - private VictorSPX m_rightVictorSPX; - - // Limit Switches - private DigitalInput m_leftLimitSwitch; - private DigitalInput m_rightLimitSwitch; - - // Clutch Solenoids - private DoubleSolenoid m_clutchSolenoidLeft; - private DoubleSolenoid m_clutchSolenoidRight; + private IClimbersIO m_climbIO; + private ClimbersIOOutputs m_outputs = new ClimbersIOOutputs(); + private ClimbersIOInputs m_Inputs = new ClimbersIOInputs(); // Member to track if the climb controls are enabled private boolean m_climbControlsEnabled = false; @@ -62,112 +57,98 @@ public enum Side { * Creates a new Climbers subsystem * @param config */ - public Climbers() { - m_leftVictorSPX = new VictorSPX(VMap.VictorSPXLeftCanID); - m_leftVictorSPX.configFactoryDefault(); - m_leftVictorSPX.setInverted(VMap.LeftInverted); - m_leftVictorSPX.setNeutralMode(NeutralMode.Brake); - m_leftVictorSPX.configOpenloopRamp(0.5); - - m_rightVictorSPX = new VictorSPX(VMap.VictorSPXRightCanID); - m_rightVictorSPX.configFactoryDefault(); - m_rightVictorSPX.setInverted(VMap.RightInverted); - m_rightVictorSPX.setNeutralMode(NeutralMode.Brake); - m_rightVictorSPX.configOpenloopRamp(0.5); - - m_leftLimitSwitch = new DigitalInput(VMap.LeftLimitSwitchDIOChannel); - m_rightLimitSwitch = new DigitalInput(VMap.RightLimitSwitchDIOChannel); - - m_clutchSolenoidLeft = - new DoubleSolenoid( - 30, - PneumaticsModuleType.REVPH, - VMap.LeftSolenoidForwardChannel, - VMap.LeftSolenoidReverseChannel - ); - m_clutchSolenoidRight = - new DoubleSolenoid( - 30, - PneumaticsModuleType.REVPH, - VMap.RightSolenoidForwardChannel, - VMap.RightSolenoidReverseChannel - ); + public ClimbersSubsystem(boolean isReal) { + if (isReal) { + m_climbIO = new ClimbersIOReal(); + } else { + m_climbIO = new ClimbersIOSim(); + } } //#region Control Methods /** - * Raises the desired climber arm - * @param side The side to raise - */ - public void raiseArm(Side side) { - if (side == Side.kLeft && !m_leftLimitSwitch.get()) { - m_leftVictorSPX.set(VictorSPXControlMode.PercentOutput, VMap.ClimberUpSpeed); - } + * Raises the desired climber arm + * @param side The side to raise + */ + public void raiseArm(Side side) { + if (side == Side.kLeft && !m_Inputs.leftLimitSwitch) { + m_outputs.leftMotorOutput = VMap.ClimberUpSpeed; + } - if (side == Side.kRight && !m_rightLimitSwitch.get()) { - m_rightVictorSPX.set(VictorSPXControlMode.PercentOutput, VMap.ClimberUpSpeed); + if (side == Side.kRight && !m_Inputs.rightLimitSwitch) { + m_outputs.rightMotorOutput = VMap.ClimberUpSpeed; + } } - } - /** - * Lowers the desired climber arm - * @param side The side to lower - * @param speed The speed to lower the arm at - */ - public void lowerArm(Side side, double speed) { - if (side == Side.kLeft) { - m_leftVictorSPX.set(VictorSPXControlMode.PercentOutput, -speed); - } + /** + * Lowers the desired climber arm + * @param side The side to lower + * @param speed The speed to lower the arm at + */ - if (side == Side.kRight) { - m_rightVictorSPX.set(VictorSPXControlMode.PercentOutput, -speed); - } - } + public void lowerArm(Side side, double speed) { + if (side == Side.kLeft) { + m_outputs.leftMotorOutput = -speed; + } - /** - * Stops the desired climber arm - * @param side - */ - public void stopArm(Side side) { - if (side == Side.kLeft) { - m_leftVictorSPX.set(VictorSPXControlMode.PercentOutput, 0); - setClutch(Side.kLeft, true); + if (side == Side.kRight) { + m_outputs.rightMotorOutput = -speed; + } } - if (side == Side.kRight) { - m_rightVictorSPX.set(VictorSPXControlMode.PercentOutput, 0); - setClutch(Side.kRight, true); - } - } + /** + * Stops the desired climber arm + * @param side + */ + public void stopArm(Side side) { + if (side == Side.kLeft) { + m_outputs.leftMotorOutput = 0; + setClutch(Side.kLeft, true); + } - /** - * Engages / disengages the desired clutch - * @param side The side to engage / disengage - * @param engaged Whether to engage or disengage the clutch - */ - public void setClutch(Side side, boolean engaged) { - if (side == Side.kLeft) { - m_clutchSolenoidLeft.set(engaged ? Value.kForward : Value.kReverse); + if (side == Side.kRight) { + m_outputs.rightMotorOutput = 0; + setClutch(Side.kRight, true); + } } - if (side == Side.kRight) { - m_clutchSolenoidRight.set(engaged ? Value.kForward : Value.kReverse); + /** + * Engages / disengages the desired clutch + * @param side The side to engage / disengage + * @param engaged Whether to engage or disengage the clutch + */ + public void setClutch(Side side, boolean engaged) { + if (side == Side.kLeft) { + m_outputs.ClutchLeft = engaged; + + } + + if (side == Side.kRight) { + m_outputs.ClutchRight = engaged; + + } } - } + @Override public void periodic() { - // d_leftLimitEntry.setBoolean(m_leftLimitSwitch.get()); - // d_rightLimitEntry.setBoolean(m_rightLimitSwitch.get()); + m_Inputs = m_climbIO.getInputs(); + m_climbIO.setOutputs(m_outputs); + + DriverDashboard.ClimberControlsActiveBox.setBoolean(m_climbControlsEnabled); // Level2 Logging + SmartDashboard.putBoolean("Climbers/ControlsEnabled", m_climbControlsEnabled); - SmartDashboard.putNumber("Climbers/LeftMotorOutput", m_leftVictorSPX.getMotorOutputPercent()); - SmartDashboard.putNumber("Climbers/RightMotorOutput", m_rightVictorSPX.getMotorOutputPercent()); - SmartDashboard.putBoolean("Climbers/LeftLimitSwitch", m_leftLimitSwitch.get()); - SmartDashboard.putBoolean("Climbers/RightLimitSwitch", m_rightLimitSwitch.get()); + // outputs + SmartDashboard.putNumber("Climbers/LeftMotorOutput", m_outputs.leftMotorOutput); + SmartDashboard.putNumber("Climbers/RightMotorOutput", m_outputs.rightMotorOutput); + // + // inputs + SmartDashboard.putBoolean("Climbers/LeftLimitSwitch", m_Inputs.leftLimitSwitch); + SmartDashboard.putBoolean("Climbers/RightLimitSwitch", m_Inputs.rightLimitSwitch); } //#endregion @@ -197,7 +178,7 @@ public Command defaultClimbingCommand( if (m_climbControlsEnabled) { // Raise Right var raiseRightArmTriggered = raiseRightArm.getAsBoolean(); - var rightLimitSwitchTriggered = m_rightLimitSwitch.get(); + var rightLimitSwitchTriggered = m_Inputs.rightLimitSwitch; if (raiseRightArmTriggered && !rightLimitSwitchTriggered) { setClutch(Side.kRight, false); raiseArm(Side.kRight); @@ -207,7 +188,7 @@ public Command defaultClimbingCommand( // Raise left var raiseLeftArmTriggered = raiseLeftArm.getAsBoolean(); - var leftLimitSwitchTriggered = m_leftLimitSwitch.get(); + var leftLimitSwitchTriggered = m_Inputs.leftLimitSwitch; if (raiseLeftArmTriggered && !leftLimitSwitchTriggered) { setClutch(Side.kLeft, false); raiseArm(Side.kLeft); @@ -245,7 +226,7 @@ public SequentialCommandGroup setArmsUpCommand() { raiseArm(Side.kRight); }) ) - .andThen(new WaitUntilCommand(() -> m_leftLimitSwitch.get() || m_rightLimitSwitch.get()).withTimeout(2)) + .andThen(new WaitUntilCommand(() -> m_Inputs.leftLimitSwitch || m_Inputs.rightLimitSwitch).withTimeout(2)) .andThen(() -> { stopArm(Side.kLeft); stopArm(Side.kRight); diff --git a/src/main/java/frc/robot/subsystems/climbers/IClimbersIO.java b/src/main/java/frc/robot/subsystems/climbers/IClimbersIO.java new file mode 100644 index 0000000..029041c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climbers/IClimbersIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.climbers; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; + +@Logged(strategy = Strategy.OPT_IN) +public interface IClimbersIO { + + + public ClimbersIOInputs getInputs(); + public void setOutputs(ClimbersIOOutputs outputs); + + +} From c086df9e1221807f74f44b858f0f13f9ce8b89e1 Mon Sep 17 00:00:00 2001 From: EthanElite Date: Tue, 17 Dec 2024 17:03:34 -0600 Subject: [PATCH 24/24] Merging intake_io to drivetrain_sim (#7) * actively working on converting intake to IO * continued implementing the interface system to the intake subsystem. * working on converting the intake subsystem to IO * finished moving the intake to the IO system. * changed Intake to IntakeSubsystem in Container * resolved errors stemming from drivetrain-sim merge --------- Co-authored-by: PoE309 Co-authored-by: zeroClearAmerican --- .vscode/settings.json | 1 + src/main/java/frc/robot/CombinedCommands.java | 10 +- src/main/java/frc/robot/Container.java | 87 +++++++-------- .../robot/subsystems/intake/IIntakeIO.java | 11 ++ .../subsystems/intake/IntakeIOInputs.java | 12 +++ .../subsystems/intake/IntakeIOOutputs.java | 8 ++ .../robot/subsystems/intake/IntakeIOReal.java | 70 ++++++++++++ .../robot/subsystems/intake/IntakeIOSim.java | 25 +++++ .../IntakeSubsystem.java} | 102 ++++++------------ 9 files changed, 201 insertions(+), 125 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IIntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOInputs.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOOutputs.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java rename src/main/java/frc/robot/subsystems/{Intake.java => intake/IntakeSubsystem.java} (60%) diff --git a/.vscode/settings.json b/.vscode/settings.json index b1d79f8..5c7be8c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -36,4 +36,5 @@ "editor.detectIndentation": false, "editor.tabSize": 2 }, + "java.import.gradle.annotationProcessing.enabled": false } diff --git a/src/main/java/frc/robot/CombinedCommands.java b/src/main/java/frc/robot/CombinedCommands.java index b901e17..97df132 100644 --- a/src/main/java/frc/robot/CombinedCommands.java +++ b/src/main/java/frc/robot/CombinedCommands.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.subsystems.Intake; +import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; public class CombinedCommands { @@ -16,7 +16,7 @@ public class CombinedCommands { * * @return */ - public SequentialCommandGroup scoreInSpeakerSequentialGroup(ShooterSubsystem shooter, Intake intake) { + public SequentialCommandGroup scoreInSpeakerSequentialGroup(ShooterSubsystem shooter, IntakeSubsystem intake) { return shooter.startShootingNoteCommand().andThen(new WaitCommand(0.75)).andThen(intake.ejectNoteCommand()) .andThen(new WaitCommand(0.75)).andThen(shooter.stopMotorsCommand()).andThen(intake.stopRollersCommand()); } @@ -26,7 +26,7 @@ public SequentialCommandGroup scoreInSpeakerSequentialGroup(ShooterSubsystem sho * * @return */ - public SequentialCommandGroup loadNoteForAmp(ShooterSubsystem shooter, Intake intake) { + public SequentialCommandGroup loadNoteForAmp(ShooterSubsystem shooter, IntakeSubsystem intake) { return Commands.runOnce(() -> intake.runIntakeRollers(-0.7)) // Eject from the intake .alongWith(Commands.runOnce(() -> shooter.runShooter(0.1))) // Load into the shooter .andThen(new WaitUntilCommand(shooter::isNoteLoaded).withTimeout(1)) // Wait until the note is loaded @@ -39,7 +39,7 @@ public SequentialCommandGroup loadNoteForAmp(ShooterSubsystem shooter, Intake in * * @return */ - public SequentialCommandGroup stopShooterAndIntakeCommand(ShooterSubsystem shooter, Intake intake) { + public SequentialCommandGroup stopShooterAndIntakeCommand(ShooterSubsystem shooter, IntakeSubsystem intake) { return shooter.stopMotorsCommand().andThen(intake.stopRollersCommand()); } @@ -48,7 +48,7 @@ public SequentialCommandGroup stopShooterAndIntakeCommand(ShooterSubsystem shoot * * @return */ - public Map getNamedCommands(ShooterSubsystem shooter, Intake intake) { + public Map getNamedCommands(ShooterSubsystem shooter, IntakeSubsystem intake) { return Map.of("Score_In_Speaker", scoreInSpeakerSequentialGroup(shooter, intake), "Load_Note_For_Amp", loadNoteForAmp(shooter, intake), "Stop_Shooter_And_Intake", stopShooterAndIntakeCommand(shooter, intake)); } diff --git a/src/main/java/frc/robot/Container.java b/src/main/java/frc/robot/Container.java index d847ac9..8c88032 100644 --- a/src/main/java/frc/robot/Container.java +++ b/src/main/java/frc/robot/Container.java @@ -16,8 +16,10 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.*; +import frc.robot.subsystems.climbers.ClimbersSubsystem; import frc.robot.maps.DriveMap; import frc.robot.subsystems.drivetrain.DrivetrainSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -37,9 +39,8 @@ public class Container { public DrivetrainSubsystem Drivetrain; @Logged(name = "Shooter", importance = Importance.CRITICAL) public ShooterSubsystem Shooter; - // public Intake Intake; - // public Climbers Climbers; - // public PwmLEDs LEDs; + public IntakeSubsystem Intake; + public ClimbersSubsystem Climbers; @Logged(name = "LEDs", importance = Importance.CRITICAL) public PwmLEDs LEDs; // public Compressor Compressor; @@ -50,7 +51,7 @@ public Container(boolean isReal) { try { DriverDashboard.init(isReal); m_driverController = new PrimeXboxController(Controls.DRIVER_PORT); - // m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); + m_operatorController = new PrimeXboxController(Controls.OPERATOR_PORT); // Create new subsystems LEDs = new PwmLEDs(); @@ -58,8 +59,8 @@ public Container(boolean isReal) { Drivetrain = new DrivetrainSubsystem(isReal, LEDs::clearForegroundPattern, LEDs::setForegroundPattern, Vision::getAllLimelightInputs); Shooter = new ShooterSubsystem(LEDs::clearForegroundPattern, LEDs::setForegroundPattern); - // Intake = new Intake(); - // Climbers = new Climbers(); + Intake = new IntakeSubsystem(isReal); + Climbers = new ClimbersSubsystem(isReal); // Compressor = new Compressor(30, PneumaticsModuleType.REVPH); // Compressor.enableDigital(); @@ -67,9 +68,9 @@ public Container(boolean isReal) { // Register the named commands from each subsystem that may be used in PathPlanner NamedCommands.registerCommands(Drivetrain.getNamedCommands()); - // NamedCommands.registerCommands(Intake.getNamedCommands()); - // NamedCommands.registerCommands(Shooter.getNamedCommands()); - // NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // + NamedCommands.registerCommands(Intake.getNamedCommands()); + NamedCommands.registerCommands(Shooter.getNamedCommands()); + NamedCommands.registerCommands(m_combinedCommands.getNamedCommands(Shooter, Intake)); // // Register the combined named commands that use multiple subsystems // Create Auto chooser and Auto tab in Shuffleboard @@ -127,16 +128,11 @@ public void configureDriverControls() { m_driverController.pov(Controls.right).onTrue(Drivetrain.setSnapToSetpointCommand(90)); // Climbers - // m_driverController.y().onTrue(Climbers.toggleClimbControlsCommand()); - // m_driverController.start().onTrue(Climbers.setArmsUpCommand()); - // Climbers.setDefaultCommand( - // Climbers.defaultClimbingCommand( - // m_driverController.button(Controls.RB), - // m_driverController.button(Controls.LB), - // () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), - // () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER) - // ) - // ); + m_driverController.y().onTrue(Climbers.toggleClimbControlsCommand()); + m_driverController.start().onTrue(Climbers.setArmsUpCommand()); + Climbers.setDefaultCommand(Climbers.defaultClimbingCommand(m_driverController.button(Controls.RB), + m_driverController.button(Controls.LB), () -> m_driverController.getRawAxis(Controls.RIGHT_TRIGGER), + () -> m_driverController.getRawAxis(Controls.LEFT_TRIGGER))); } /** @@ -144,36 +140,27 @@ public void configureDriverControls() { */ public void configureOperatorControls() { // Intake ======================================== - // m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle - // in/out - - // m_operatorController // When the trigger is pressed, intake a note at a variable speed - // .leftTrigger(0.1) - // .whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) - // .onFalse(Intake.stopRollersCommand()); - - // m_operatorController // When the trigger is pressed, eject a note at a constant speed - // .rightTrigger(0.1) - // .whileTrue(Intake.ejectNoteCommand()) - // .onFalse(Intake.stopRollersCommand()); - - // // Shooter ======================================== - // m_operatorController // Toggle the elevation of the shooter - // .rightBumper() - // .onTrue(Shooter.toggleElevationCommand()); - - // m_operatorController // Runs only the shooter motors at a constant speed to score in the amp - // .x() - // .whileTrue(Shooter.startShootingNoteCommand()) - // .onFalse(Shooter.stopMotorsCommand()); - - // // Combined shooter and intake commands =========== - // m_operatorController // score in speaker - // .b() - // .onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); - - // m_operatorController // Run sequence to load a note into the shooter for scoring in the amp - // .y() - // .onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); + m_operatorController.a().onTrue(Intake.toggleIntakeInAndOutCommand()); // Set intake angle in/out + + m_operatorController // When the trigger is pressed, intake a note at a variable speed + .leftTrigger(0.1).whileTrue(Intake.runRollersAtSpeedCommand(() -> m_operatorController.getLeftTriggerAxis())) + .onFalse(Intake.stopRollersCommand()); + + m_operatorController // When the trigger is pressed, eject a note at a constant speed + .rightTrigger(0.1).whileTrue(Intake.ejectNoteCommand()).onFalse(Intake.stopRollersCommand()); + + // Shooter ======================================== + m_operatorController // Toggle the elevation of the shooter + .rightBumper().onTrue(Shooter.toggleElevationCommand()); + + m_operatorController // Runs only the shooter motors at a constant speed to score in the amp + .x().whileTrue(Shooter.startShootingNoteCommand()).onFalse(Shooter.stopMotorsCommand()); + + // Combined shooter and intake commands =========== + m_operatorController // score in speaker + .b().onTrue(m_combinedCommands.scoreInSpeakerSequentialGroup(Shooter, Intake)); + + m_operatorController // Run sequence to load a note into the shooter for scoring in the amp + .y().onTrue(m_combinedCommands.loadNoteForAmp(Shooter, Intake)); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IIntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IIntakeIO.java new file mode 100644 index 0000000..9d4a970 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IIntakeIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.intake; + +public interface IIntakeIO { + public IntakeIOInputs getInputs(); + + public void setOutputs(IntakeIOOutputs outputs); + + public void stopAngleMotors(); + + public void stopRollerMotors(); +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOInputs.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOInputs.java new file mode 100644 index 0000000..86be6c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOInputs.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.intake; + +public class IntakeIOInputs { + + public double AngleStartPoint; + public double intakeMotorLeftPosition = 0; + public double intakeMotorRightPosition = 0; + public boolean topLimitSwitch; + public boolean bottomLimitSwitch; + + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOOutputs.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOOutputs.java new file mode 100644 index 0000000..1fd8c32 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOOutputs.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.intake; + +public class IntakeIOOutputs { + + public double leftMotorOutput = 0; + public double rightMotorOutput = 0; + public double rollerMotorOutput = 0; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java new file mode 100644 index 0000000..fba46d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -0,0 +1,70 @@ +package frc.robot.subsystems.intake; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.intake.IntakeSubsystem.VMap; + +public class IntakeIOReal implements IIntakeIO { + private DigitalInput m_topLimitSwitch; //input + private DigitalInput m_bottomLimitSwitch; + private CANSparkMax m_rollers; //output + private CANSparkMax m_angleLeft; //output + private CANSparkMax m_angleRight; //output + + private IntakeIOInputs m_inputs; + + + public IntakeIOReal() { + m_inputs = new IntakeIOInputs(); + + m_topLimitSwitch = new DigitalInput(VMap.TopLimitSwitchChannel); + m_bottomLimitSwitch = new DigitalInput(VMap.BottomLimitSwitchChannel); + + m_rollers = new CANSparkMax(VMap.RollersCanId, MotorType.kBrushless); + m_rollers.restoreFactoryDefaults(); + m_rollers.setInverted(VMap.RollersInverted); + m_rollers.setSmartCurrentLimit(40, 50); + // m_rollers.setOpenLoopRampRate(0.250); + + m_angleLeft = new CANSparkMax(VMap.NeoLeftCanId, MotorType.kBrushless); + m_angleLeft.restoreFactoryDefaults(); + m_angleLeft.setInverted(VMap.NeoLeftInverted); + m_angleLeft.setSmartCurrentLimit(40, 60); + + m_angleRight = new CANSparkMax(VMap.NeoRightCanId, MotorType.kBrushless); + m_angleRight.restoreFactoryDefaults(); + m_angleRight.setInverted(VMap.NeoRightInverted); + m_angleRight.setSmartCurrentLimit(40, 60); + + m_inputs.AngleStartPoint = m_angleRight.getEncoder().getPosition(); + SmartDashboard.putNumber("Intake/AngleStartPoint", m_inputs.AngleStartPoint); + } + + @Override + public IntakeIOInputs getInputs() { + m_inputs.intakeMotorLeftPosition = m_angleLeft.getEncoder().getPosition(); + m_inputs.intakeMotorRightPosition = m_angleRight.getEncoder().getPosition(); + m_inputs.bottomLimitSwitch = m_bottomLimitSwitch.get(); + m_inputs.topLimitSwitch = m_topLimitSwitch.get(); + + return m_inputs; + } + @Override + public void setOutputs(IntakeIOOutputs outputs) { + m_angleLeft.set(outputs.leftMotorOutput); + m_angleRight.set(outputs.rightMotorOutput); + m_rollers.set(outputs.rollerMotorOutput); + } + + public void stopAngleMotors() { + m_angleLeft.stopMotor(); + m_angleRight.stopMotor(); + } + + public void stopRollerMotors() { + m_rollers.stopMotor(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..a23aabe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.intake; + +public class IntakeIOSim implements IIntakeIO { + + @Override + public IntakeIOInputs getInputs() { + return new IntakeIOInputs(); + } + + @Override + public void setOutputs(IntakeIOOutputs outputs) { + + } + + @Override + public void stopAngleMotors() { + + } + + @Override + public void stopRollerMotors() { + + } + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java similarity index 60% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index a3dd8cd..e30c3d8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -1,12 +1,8 @@ -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkLowLevel.MotorType; +package frc.robot.subsystems.intake; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -15,7 +11,7 @@ import java.util.function.DoubleSupplier; import prime.control.PrimePIDConstants; -public class Intake extends SubsystemBase { +public class IntakeSubsystem extends SubsystemBase { public static class VMap { @@ -31,48 +27,30 @@ public static class VMap { public static final int BottomLimitSwitchChannel = 5; } - private DigitalInput m_topLimitSwitch; - private DigitalInput m_bottomLimitSwitch; - - private CANSparkMax m_rollers; - private CANSparkMax m_angleLeft; - private CANSparkMax m_angleRight; - + private boolean m_angleToggledIn = true; + private Debouncer m_angleToggleDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); //output private PIDController m_anglePid; - private double m_angleStartPoint; - public boolean m_angleToggledIn; - private Debouncer m_angleToggleDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + + private IIntakeIO m_intakeIO; +private IntakeIOInputs m_inputs = new IntakeIOInputs(); + private IntakeIOOutputs m_outputs = new IntakeIOOutputs(); /** * Creates a new Intake subsystem * @param robotConfig */ - public Intake() { + public IntakeSubsystem(boolean isReal) { setName("Intake"); - m_topLimitSwitch = new DigitalInput(VMap.TopLimitSwitchChannel); - m_bottomLimitSwitch = new DigitalInput(VMap.BottomLimitSwitchChannel); - - m_rollers = new CANSparkMax(VMap.RollersCanId, MotorType.kBrushless); - m_rollers.restoreFactoryDefaults(); - m_rollers.setInverted(VMap.RollersInverted); - m_rollers.setSmartCurrentLimit(40, 50); - // m_rollers.setOpenLoopRampRate(0.250); - - m_angleLeft = new CANSparkMax(VMap.NeoLeftCanId, MotorType.kBrushless); - m_angleLeft.restoreFactoryDefaults(); - m_angleLeft.setInverted(VMap.NeoLeftInverted); - m_angleLeft.setSmartCurrentLimit(40, 60); - - m_angleRight = new CANSparkMax(VMap.NeoRightCanId, MotorType.kBrushless); - m_angleRight.restoreFactoryDefaults(); - m_angleRight.setInverted(VMap.NeoRightInverted); - m_angleRight.setSmartCurrentLimit(40, 60); + if (isReal) { + m_intakeIO = new IntakeIOReal(); + } else { + m_intakeIO = new IntakeIOSim(); + } - m_angleStartPoint = getPositionRight(); - SmartDashboard.putNumber("Intake/AngleStartPoint", m_angleStartPoint); + SmartDashboard.putNumber("Intake/AngleStartPoint", m_inputs.AngleStartPoint); m_anglePid = VMap.IntakeAnglePid.createPIDController(0.02); - m_anglePid.setSetpoint(m_angleStartPoint); + m_anglePid.setSetpoint(m_inputs.AngleStartPoint); m_angleToggledIn = true; // Set the default command for the subsystem so that it runs the PID loop @@ -81,28 +59,13 @@ public Intake() { //#region Control Methods - /** - * Gets the current position of the Intake Angle from the right NEO's encoder - * @return - */ - public double getPositionRight() { - return m_angleRight.getEncoder().getPosition(); - } - - /** - * Gets the current position of the Intake Angle from the left NEO's encoder - * @return - */ - public double getPositionLeft() { - return m_angleLeft.getEncoder().getPosition(); - } - + /** * Runs intake rollers at a given speed * @param speed */ public void runIntakeRollers(double speed) { - m_rollers.set(speed); + m_outputs.rollerMotorOutput = speed; } /** @@ -110,8 +73,8 @@ public void runIntakeRollers(double speed) { * @param speed */ public void setAngleMotorSpeed(double speed) { - m_angleLeft.set(-speed); - m_angleRight.set(speed); + m_outputs.leftMotorOutput = -speed; + m_outputs.rightMotorOutput = speed; } /** @@ -119,18 +82,18 @@ public void setAngleMotorSpeed(double speed) { * @param positionSetpoint */ public void setIntakeRotation() { - var currentPosition = getPositionRight(); - var setpoint = m_angleToggledIn ? m_angleStartPoint : (m_angleStartPoint - VMap.PositionDelta); + var currentPosition = m_inputs.intakeMotorRightPosition; + var setpoint = m_angleToggledIn ? m_inputs.AngleStartPoint : (m_inputs.AngleStartPoint - VMap.PositionDelta); SmartDashboard.putNumber("Intake/AngleSetpoint", setpoint); var pidOutput = m_anglePid.calculate(currentPosition, setpoint); SmartDashboard.putNumber("Intake/AnglePIDOutput", pidOutput); // artificial limits - if (currentPosition < m_angleStartPoint && pidOutput > 0 && !m_topLimitSwitch.get()) { + if (currentPosition < m_inputs.AngleStartPoint && pidOutput > 0 && !m_inputs.topLimitSwitch) { setAngleMotorSpeed(MathUtil.clamp(pidOutput, 0, 1)); } else if ( - currentPosition > (m_angleStartPoint - VMap.PositionDelta) && pidOutput < 0 && !m_bottomLimitSwitch.get() + currentPosition > (m_inputs.AngleStartPoint - VMap.PositionDelta) && pidOutput < 0 && !m_inputs.bottomLimitSwitch ) { setAngleMotorSpeed(MathUtil.clamp(pidOutput, -1, 0)); } else { @@ -143,15 +106,15 @@ public void setIntakeRotation() { @Override public void periodic() { // Level2 Logging - SmartDashboard.putBoolean("Intake/ToggledIn", m_angleToggledIn); + // SmartDashboard.putBoolean("Intake/ToggledIn", m_angleToggledIn); - SmartDashboard.putNumber("Intake/ArmPositionRight", getPositionRight()); - SmartDashboard.putNumber("Intake/ArmPositionLeft", getPositionLeft()); + // SmartDashboard.putNumber("Intake/ArmPositionRight", getPositionRight()); + // SmartDashboard.putNumber("Intake/ArmPositionLeft", getPositionLeft()); - SmartDashboard.putNumber("Intake/RightMotorOutput", m_angleRight.get()); - SmartDashboard.putNumber("Intake/LeftMotorOutput", m_angleLeft.get()); + // SmartDashboard.putNumber("Intake/RightMotorOutput", m_angleRight.get()); + // SmartDashboard.putNumber("Intake/LeftMotorOutput", m_angleLeft.get()); - SmartDashboard.putNumber("Intake/RollersOutput", m_rollers.get()); + // SmartDashboard.putNumber("Intake/RollersOutput", m_rollers.get()); } //#region Commands @@ -213,8 +176,7 @@ public Command toggleIntakeInAndOutCommand() { */ public Command stopArmMotorsCommand() { return Commands.runOnce(() -> { - m_angleLeft.stopMotor(); - m_angleRight.stopMotor(); + m_intakeIO.stopAngleMotors(); }); } @@ -224,7 +186,7 @@ public Command stopArmMotorsCommand() { */ public Command stopRollersCommand() { return Commands.runOnce(() -> { - m_rollers.stopMotor(); + m_intakeIO.stopRollerMotors(); }); }