diff --git a/build.gradle b/build.gradle index 027f0d98..a730a6ab 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" id 'com.diffplug.spotless' version '6.11.0' id "com.peterabeles.gversion" version "1.10" } diff --git a/simgui.json b/simgui.json index f64a325a..384c947e 100644 --- a/simgui.json +++ b/simgui.json @@ -11,6 +11,13 @@ "/SmartDashboard/SystemStatus/SwerveModule2/SystemCheck": "Command", "/SmartDashboard/SystemStatus/SwerveModule3/SystemCheck": "Command", "/SmartDashboard/VisionSystemSim-simCamera/Sim Field": "Field2d" + }, + "windows": { + "/Shuffleboard/MAIN/SendableChooser[0]": { + "window": { + "visible": true + } + } } }, "NetworkTables Info": { diff --git a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java index 6912b10d..ce287fb1 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java +++ b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java @@ -104,7 +104,7 @@ public Drivetrain(DrivetrainIO io) { this.autoThetaController.enableContinuousInput(-Math.PI, Math.PI); - this.isFieldRelative = false; + this.isFieldRelative = true; // based on testing we can drive in turbo mode all the time this.isTurbo = true; @@ -400,8 +400,12 @@ public void driveFacingAngle( xVelocity *= slowModeMultiplier; yVelocity *= slowModeMultiplier; } - - this.io.driveFieldRelativeFacingAngle(xVelocity, yVelocity, targetDirection, isOpenLoop); + int allianceMultiplier = this.alliance == Alliance.Blue ? 1 : -1; + this.io.driveFieldRelativeFacingAngle( + xVelocity * allianceMultiplier, + yVelocity * allianceMultiplier, + targetDirection, + isOpenLoop); } /** @@ -737,7 +741,7 @@ private String getSwerveLocation(int swerveModuleNumber) { } /* - * Checks the swerve module to see if its velocity and rotation are moving as expected + * Checks the swerve module to see if its velocity and rotation are moving as expected * @param swerveModuleNumber the swerve module number to check */ @@ -748,7 +752,7 @@ private void checkSwerveModule( double velocityTarget, double velocityTolerance) { - Boolean isOffset = false; + Boolean isOffset = false; // Check to see if the direction is rotated properly // steerAbsolutePositionDeg is a value that is between (-180, 180] @@ -758,7 +762,7 @@ private void checkSwerveModule( && this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg < angleTarget + angleTolerance) { } - + // check if angle is in threshold +- 180 else if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg > angleTarget - angleTolerance - 180 @@ -783,8 +787,8 @@ else if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg + " is: " + inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg); } - - // Checks the velocity of the swerve module depending on if there is an offset + + // Checks the velocity of the swerve module depending on if there is an offset if (!isOffset) { if (inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec @@ -802,7 +806,7 @@ else if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg + " is: " + inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec); } - } else { // if there is an offset, check the velocity in the opposite direction + } else { // if there is an offset, check the velocity in the opposite direction if (inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec < -(velocityTarget - velocityTolerance) && inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java index 86039ff6..3d42dad0 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java @@ -284,6 +284,11 @@ public DrivetrainIOCTRE() { driveFacingAngleThetaKi.get(), driveFacingAngleThetaKd.get()); this.driveFacingAngleRequest.HeadingController.enableContinuousInput(0, Math.PI * 2); + + // always define 0° (towards the red alliance) as "forward"; the Drivetrain subsystem handles + // the definition of forward based on the current alliance + this.driveFacingAngleRequest.ForwardReference = SwerveRequest.ForwardReference.RedAlliance; + this.driveFieldCentricRequest.ForwardReference = SwerveRequest.ForwardReference.RedAlliance; } @Override diff --git a/src/main/java/frc/lib/team3061/sim/ArmSystemSim.java b/src/main/java/frc/lib/team3061/sim/ArmSystemSim.java new file mode 100644 index 00000000..4417e6fe --- /dev/null +++ b/src/main/java/frc/lib/team3061/sim/ArmSystemSim.java @@ -0,0 +1,125 @@ +package frc.lib.team3061.sim; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.Constants; +import org.littletonrobotics.junction.Logger; + +public class ArmSystemSim { + + private TalonFX motor; + private CANcoder encoder; + private TalonFXSimState motorSimState; + private CANcoderSimState encoderSimState; + private SingleJointedArmSim systemSim; + private double sensorToMechanismRatio; + private double rotorToSensorRatio; + private String subsystemName; + + // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. + private Mechanism2d mech2d; + private MechanismRoot2d armPivot; + private MechanismLigament2d armTower; + private MechanismLigament2d arm; + + public ArmSystemSim( + TalonFX motor, + CANcoder encoder, + boolean motorInverted, + double sensorToMechanismRatio, + double rotorToSensorRatio, + double length, + double mass, + double minAngle, + double maxAngle, + double startingAngle, + String subsystemName) { + if (Constants.getMode() != Constants.Mode.SIM) { + return; + } + + this.motor = motor; + this.encoder = encoder; + this.sensorToMechanismRatio = sensorToMechanismRatio; + this.rotorToSensorRatio = rotorToSensorRatio; + + this.motorSimState = this.motor.getSimState(); + this.motorSimState.Orientation = + motorInverted + ? ChassisReference.Clockwise_Positive + : ChassisReference.CounterClockwise_Positive; + this.encoderSimState = this.encoder.getSimState(); + this.encoderSimState.Orientation = + motorInverted + ? ChassisReference.Clockwise_Positive + : ChassisReference.CounterClockwise_Positive; + + this.systemSim = + new SingleJointedArmSim( + DCMotor.getFalcon500Foc(1), + sensorToMechanismRatio * rotorToSensorRatio, + SingleJointedArmSim.estimateMOI(length, mass), + length, + minAngle, + maxAngle, + true, + startingAngle); + + this.mech2d = new Mechanism2d(1, 1); + this.armPivot = mech2d.getRoot("ArmPivot", 0.5, 0.5); + this.armTower = armPivot.append(new MechanismLigament2d("ArmTower", .3, -90)); + this.arm = + armPivot.append( + new MechanismLigament2d("Arm", length, startingAngle, 6, new Color8Bit(Color.kYellow))); + this.armTower.setColor(new Color8Bit(Color.kBlue)); + + this.subsystemName = subsystemName; + } + + public void updateSim() { + if (Constants.getMode() != Constants.Mode.SIM) { + return; + } + + // update the sim states supply voltage based on the simulated battery + this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + this.encoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + // update the input voltages of the models based on the outputs of the simulated TalonFXs + double motorVoltage = this.motorSimState.getMotorVoltage(); + this.systemSim.setInput(motorVoltage); + + // update the models + this.systemSim.update(Constants.LOOP_PERIOD_SECS); + + // update the simulated TalonFX based on the model outputs + double mechanismRadians = this.systemSim.getAngleRads(); + double mechanismRotations = mechanismRadians / (2 * Math.PI); + double sensorRotations = mechanismRotations * this.sensorToMechanismRatio; + double motorRotations = sensorRotations * this.rotorToSensorRatio; + double mechanismRadiansPerSec = this.systemSim.getVelocityRadPerSec(); + double mechanismRPS = mechanismRadiansPerSec / (2 * Math.PI); + double encoderRPS = mechanismRPS * this.sensorToMechanismRatio; + double motorRPS = encoderRPS * this.rotorToSensorRatio; + this.motorSimState.setRawRotorPosition(motorRotations); + this.motorSimState.setRotorVelocity(motorRPS); + this.encoderSimState.setRawPosition(sensorRotations); + this.encoderSimState.setVelocity(encoderRPS); + + // Update the Mechanism Arm angle based on the simulated arm angle + arm.setAngle(Units.radiansToDegrees(mechanismRadians)); + Logger.recordOutput(subsystemName + "/ArmSim", mech2d); + } +} diff --git a/src/main/java/frc/lib/team3061/sim/VelocitySystemSim.java b/src/main/java/frc/lib/team3061/sim/VelocitySystemSim.java new file mode 100644 index 00000000..210fa866 --- /dev/null +++ b/src/main/java/frc/lib/team3061/sim/VelocitySystemSim.java @@ -0,0 +1,58 @@ +package frc.lib.team3061.sim; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import frc.robot.Constants; + +public class VelocitySystemSim { + + private TalonFX motor; + private TalonFXSimState motorSimState; + private LinearSystemSim systemSim; + private double gearRatio; + + public VelocitySystemSim( + TalonFX motor, boolean motorInverted, double kV, double kA, double gearRatio) { + this.motor = motor; + this.gearRatio = gearRatio; + + if (Constants.getMode() != Constants.Mode.SIM) { + return; + } + + this.motorSimState = this.motor.getSimState(); + this.motorSimState.Orientation = + motorInverted + ? ChassisReference.Clockwise_Positive + : ChassisReference.CounterClockwise_Positive; + + this.systemSim = new LinearSystemSim<>(LinearSystemId.identifyVelocitySystem(kV, kA)); + } + + public void updateSim() { + if (Constants.getMode() != Constants.Mode.SIM) { + return; + } + + // update the sim states supply voltage based on the simulated battery + this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + // update the input voltages of the models based on the outputs of the simulated TalonFXs + this.systemSim.setInput(this.motorSimState.getMotorVoltage()); + + // update the models + this.systemSim.update(Constants.LOOP_PERIOD_SECS); + + // update the simulated TalonFX based on the model outputs + double mechanismRadiansPerSec = this.systemSim.getOutput(0); + double motorRPS = mechanismRadiansPerSec * this.gearRatio / (2 * Math.PI); + double motorRotations = motorRPS * Constants.LOOP_PERIOD_SECS; + this.motorSimState.addRotorPosition(motorRotations); + this.motorSimState.setRotorVelocity(motorRPS); + } +} diff --git a/src/main/java/frc/lib/team3061/vision/Vision.java b/src/main/java/frc/lib/team3061/vision/Vision.java index 417169d1..e3441c6e 100644 --- a/src/main/java/frc/lib/team3061/vision/Vision.java +++ b/src/main/java/frc/lib/team3061/vision/Vision.java @@ -51,10 +51,14 @@ public class Vision extends SubsystemBase { private RobotOdometry odometry; private final TunableNumber poseDifferenceThreshold = new TunableNumber("Vision/VisionPoseThreshold", POSE_DIFFERENCE_THRESHOLD_METERS); - private final TunableNumber stdDevSlope = new TunableNumber("Vision/stdDevSlope", 0.10); - private final TunableNumber stdDevPower = new TunableNumber("Vision/stdDevPower", 2.0); + private final TunableNumber stdDevSlopeDistance = + new TunableNumber("Vision/StdDevSlopeDistance", 0.10); + private final TunableNumber stdDevPowerDistance = + new TunableNumber("Vision/stdDevPowerDistance", 2.0); private final TunableNumber stdDevMultiTagFactor = new TunableNumber("Vision/stdDevMultiTagFactor", 0.2); + private final TunableNumber stdDevFactorAmbiguity = + new TunableNumber("Vision/StdDevSlopeFactorAmbiguity", 1.0); /** * Create a new Vision subsystem. The number of VisionIO objects passed to the constructor must @@ -148,15 +152,19 @@ private void processNewVisionData(int i) { RobotConfig.getInstance().getRobotToCameraTransforms()[i].inverse()); Pose2d estimatedRobotPose2d = estimatedRobotPose3d.toPose2d(); - // only update the pose estimator if the vision subsystem is enabled - if (isEnabled) { + // only update the pose estimator if the vision subsystem is enabled and vision's estimated + // pose is within the specified tolerance of the current pose + if (isEnabled + && estimatedRobotPose2d + .getTranslation() + .getDistance(odometry.getEstimatedPosition().getTranslation()) + < MAX_POSE_DIFFERENCE_METERS) { // when updating the pose estimator, specify standard deviations based on the distance // from the robot to the AprilTag (the greater the distance, the less confident we are // in the measurement) + Matrix stdDev = getStandardDeviations(i, estimatedRobotPose2d, ios[i].minAmbiguity); odometry.addVisionMeasurement( - estimatedRobotPose2d, - ios[i].estimatedCameraPoseTimestamp, - getStandardDeviations(i, estimatedRobotPose2d)); + estimatedRobotPose2d, ios[i].estimatedCameraPoseTimestamp, stdDev); isVisionUpdating = true; } @@ -241,8 +249,11 @@ public boolean posesHaveConverged() { * * @param estimatedPose The estimated pose to guess standard deviations for. */ - private Matrix getStandardDeviations(int index, Pose2d estimatedPose) { - Matrix estStdDevs = VecBuilder.fill(1, 1, 2); + private Matrix getStandardDeviations( + int index, Pose2d estimatedPose, double minAmbiguity) { + // The gyro is very accurate; so, rely on the vision pose estimation primarily for x and y + // position and not for rotation. + Matrix estStdDevs = VecBuilder.fill(1, 1, 10); int numTags = 0; double avgDist = 0; for (int tagID = 0; tagID < ios[index].tagsSeen.length; tagID++) { @@ -264,9 +275,14 @@ private Matrix getStandardDeviations(int index, Pose2d estimatedPose) { if (numTags == 1 && avgDist > MAX_DISTANCE_TO_TARGET_METERS) { estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); } else { - estStdDevs = estStdDevs.times(stdDevSlope.get() * (Math.pow(avgDist, stdDevPower.get()))); + estStdDevs = + estStdDevs.times( + stdDevSlopeDistance.get() * (Math.pow(avgDist, stdDevPowerDistance.get()))); } + // Adjust standard deviations based on the ambiguity of the pose + estStdDevs = estStdDevs.times(stdDevFactorAmbiguity.get() * minAmbiguity / MAXIMUM_AMBIGUITY); + return estStdDevs; } } diff --git a/src/main/java/frc/lib/team3061/vision/VisionConstants.java b/src/main/java/frc/lib/team3061/vision/VisionConstants.java index 87d3eba7..b73d7c2a 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionConstants.java +++ b/src/main/java/frc/lib/team3061/vision/VisionConstants.java @@ -22,7 +22,7 @@ private VisionConstants() { // the maximum distance between the robot's pose derived from the target and the current robot's // estimated pose for the target to be used to update the robot's pose (essentially, always use a // valid target to update the robot's pose) - public static final double MAX_POSE_DIFFERENCE_METERS = 1000.0; + public static final double MAX_POSE_DIFFERENCE_METERS = 2.0; // the maximum distance between the robot and the target, for the target to be used to update the // robot's pose diff --git a/src/main/java/frc/lib/team3061/vision/VisionIO.java b/src/main/java/frc/lib/team3061/vision/VisionIO.java index 87a942ac..1e9ea5cd 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIO.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIO.java @@ -18,6 +18,7 @@ public static class VisionIOInputs { double estimatedCameraPoseTimestamp = 0.0; boolean[] tagsSeen = new boolean[] {}; double lastCameraTimestamp = 0.0; + double minAmbiguity = 0.0; } /** diff --git a/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java b/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java index a5f8c17c..b007baaf 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java @@ -1,5 +1,7 @@ package frc.lib.team3061.vision; +import static frc.lib.team3061.vision.VisionConstants.*; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Transform3d; import frc.lib.team6328.util.Alert; @@ -9,6 +11,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; /** * PhotonVision-based implementation of the VisionIO interface. @@ -56,6 +59,14 @@ public void updateInputs(VisionIOInputs inputs) { boolean newResult = Math.abs(latestTimestamp - this.lastTimestamp) > 1e-5; if (newResult) { + double minAmbiguity = 10.0; + for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { + if (target.getPoseAmbiguity() < minAmbiguity) { + minAmbiguity = target.getPoseAmbiguity(); + } + } + inputs.minAmbiguity = minAmbiguity; + visionEstimate.ifPresent( estimate -> { inputs.estimatedCameraPose = estimate.estimatedPose; diff --git a/src/main/java/frc/lib/team6328/util/FieldConstants.java b/src/main/java/frc/lib/team6328/util/FieldConstants.java index 6df55afa..73393769 100644 --- a/src/main/java/frc/lib/team6328/util/FieldConstants.java +++ b/src/main/java/frc/lib/team6328/util/FieldConstants.java @@ -7,45 +7,145 @@ package frc.lib.team6328.util; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; @java.lang.SuppressWarnings({"java:S1118", "java:S115", "java:S2386"}) /** * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets - * of corners start in the lower left moving clockwise. + * of corners start in the lower left moving clockwise. All units in Meters
+ *
* *

All translations and poses are stored with the origin at the rightmost point on the BLUE - * ALLIANCE wall. Use the {@link #flipForRedSide(Translation2d)} and {@link #flipForRedSide(Pose2d)} - * methods to flip these values to get the red alliance version. + * ALLIANCE wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib) */ -public final class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(651.25); - public static final double fieldWidth = Units.inchesToMeters(323.25); - public static final double tapeWidth = Units.inchesToMeters(2.0); - - /** - * Flips a translation for a point on the blue side of the field to the corresponding point on the - * red side. By default, all translations and poses in {@link FieldConstants} are stored with the - * origin at the rightmost point on the BLUE ALLIANCE wall. - */ +public class FieldConstants { + public static final double fieldLength = Units.inchesToMeters(651.223); + public static final double fieldWidth = Units.inchesToMeters(323.277); + + public static final double blueWingX = Units.inchesToMeters(229.201); + public static final double redWingX = fieldLength - blueWingX; + public static final double bluePodiumX = Units.inchesToMeters(126.75); + public static final double redPodiumX = fieldLength - bluePodiumX; + public static final double blueStartingLineX = Units.inchesToMeters(74.111); + public static final double redStartingLineX = fieldLength - blueStartingLineX; + + public static final Translation2d blueAmpCenter = + new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + public static final Translation2d redAmpCenter = FieldConstants.flipForRedSide(blueAmpCenter); + + /** Staging locations for each note */ + public static final class StagingLocations { + public static final double centerlineX = fieldLength / 2.0; + + public static final double centerlineFirstY = Units.inchesToMeters(29.638); + private static final double centerlineSeparationY = Units.inchesToMeters(66); + private static final double blueSpikeX = Units.inchesToMeters(114.0); + // should be right + private static final double blueSpikeFirstY = Units.inchesToMeters(161.638); + private static final double blueSpikeSeparationY = Units.inchesToMeters(57); + + // Notes in center + public static final Translation2d[] centerlineTranslations = new Translation2d[5]; + public static final Translation2d[] blueSpikeTranslations = new Translation2d[3]; + public static final Translation2d[] redSpikeTranslations = new Translation2d[3]; + + static { + for (int i = 0; i < centerlineTranslations.length; i++) { + centerlineTranslations[i] = + new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); + } + } + + static { + for (int i = 0; i < blueSpikeTranslations.length; i++) { + blueSpikeTranslations[i] = + new Translation2d(blueSpikeX, blueSpikeFirstY + (i * blueSpikeSeparationY)); + } + } + + static { + for (int i = 0; i < redSpikeTranslations.length; i++) { + redSpikeTranslations[i] = FieldConstants.flipForRedSide(blueSpikeTranslations[i]); + } + } + } + + /** Each corner of the speaker * */ + public static final class BlueSpeaker { + + /** Center of the speaker opening (blue alliance) */ + public static final Pose2d blueCenterSpeakerOpening = + new Pose2d(0.0, fieldWidth - Units.inchesToMeters(104.0), new Rotation2d()); + + // corners (blue alliance origin) + public static final Translation3d blueTopRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(13.091)); + + public static final Translation3d blueTopLeftSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static final Translation3d blueBottomRightSpeaker = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static final Translation3d blueBottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + } + + public static final class RedSpeaker { + /** Center of the speaker opening (blue alliance) */ + public static final Pose2d redCenterSpeakerOpening = + flipForRedSide(BlueSpeaker.blueCenterSpeakerOpening); + + // corners (red alliance origin) + public static final Translation3d redTopRightSpeaker = + FieldConstants.flipForRedSide(BlueSpeaker.blueTopLeftSpeaker); + public static final Translation3d redTopLeftSpeaker = + FieldConstants.flipForRedSide(BlueSpeaker.blueTopRightSpeaker); + public static final Translation3d redBottomRightSpeaker = + FieldConstants.flipForRedSide(BlueSpeaker.blueBottomLeftSpeaker); + public static final Translation3d redBottomLeftSpeaker = + FieldConstants.flipForRedSide(BlueSpeaker.blueBottomRightSpeaker); + } + + public static class RedSource { + public static final Translation2d redSourceCenter = + new Translation2d(Units.inchesToMeters(35.6247), Units.inchesToMeters(21.9704)); + public static final Pose2d redSourcePose = + new Pose2d(redSourceCenter, new Rotation2d(2.8163 - (Math.PI) / 2)); + } + + public static class BlueSource { + public static final Translation2d blueSourceCenter = flipForRedSide(RedSource.redSourceCenter); + public static final Pose2d blueSourcePose = flipForRedSide(RedSource.redSourcePose); + } + + public static final double aprilTagWidth = Units.inchesToMeters(6.50); + + public static Translation3d flipForRedSide(Translation3d translation) { + return new Translation3d( + fieldLength - translation.getX(), translation.getY(), translation.getZ()); + } + public static Translation2d flipForRedSide(Translation2d translation) { return new Translation2d(fieldLength - translation.getX(), translation.getY()); } - /** - * Flips a pose from the blue side of the field to the corresponding point on the red side of the - * field. By default, all translations and poses in {@link FieldConstants} are stored with the - * origin at the rightmost point on the BLUE ALLIANCE wall. - */ - public static Pose2d flipForRedSide(Pose2d pose, boolean rotate180) { - int flipSign = rotate180 ? -1 : 0; + public static Pose2d flipForRedSide(Pose2d pose) { return new Pose2d( - fieldLength - pose.getX(), - pose.getY(), - new Rotation2d(flipSign * pose.getRotation().getCos(), pose.getRotation().getSin())); + fieldLength - pose.getX(), pose.getY(), new Rotation2d(Math.PI).minus(pose.getRotation())); + } + + public static Rotation2d flipForRedSide(Rotation2d rotation) { + return new Rotation2d(Math.PI).minus(rotation); } } diff --git a/src/main/java/frc/robot/Field2d.java b/src/main/java/frc/robot/Field2d.java index 505b5f56..f21629d6 100644 --- a/src/main/java/frc/robot/Field2d.java +++ b/src/main/java/frc/robot/Field2d.java @@ -6,8 +6,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.lib.team3061.RobotConfig; import frc.lib.team3061.drivetrain.Drivetrain; +import frc.lib.team6328.util.FieldConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.HashSet; @@ -29,6 +32,8 @@ public class Field2d { private Region2d[] regions; + private Alliance alliance; + /** * Get the singleton instance of the Field2d class. * @@ -185,4 +190,41 @@ private List breadthFirstSearch(Region2d start, Region2d end) { } return new ArrayList<>(); } + + /** + * This method should be invoked once the alliance color is known. Refer to the RobotContainer's + * checkAllianceColor method for best practices on when to check the alliance's color. The + * alliance color is needed when running auto paths as those paths are always defined for + * blue-alliance robots and need to be flipped for red-alliance robots. + * + * @param newAlliance the new alliance color + */ + public void updateAlliance(DriverStation.Alliance newAlliance) { + this.alliance = newAlliance; + } + + /** + * Get the alliance color. + * + * @return the alliance color + */ + public Alliance getAlliance() { + return alliance; + } + + public Pose2d getAllianceSpeakerCenter() { + if (alliance == Alliance.Blue) { + return FieldConstants.BlueSpeaker.blueCenterSpeakerOpening; + } else { + return FieldConstants.RedSpeaker.redCenterSpeakerOpening; + } + } + + public Pose2d getOpponentSpeakerCenter() { + if (alliance == Alliance.Blue) { + return FieldConstants.RedSpeaker.redCenterSpeakerOpening; + } else { + return FieldConstants.BlueSpeaker.blueCenterSpeakerOpening; + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 555f5d13..7a73d8be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -533,6 +534,23 @@ private void configureDrivetrainCommands() { ? Rotation2d.fromDegrees(0.0) : Rotation2d.fromDegrees(180.0))); + oi.getLockToSpeakerButton() + .whileTrue( + new TeleopSwerve( + drivetrain, + oi::getTranslateX, + oi::getTranslateY, + () -> { + Transform2d translation = + new Transform2d( + Field2d.getInstance().getAllianceSpeakerCenter().getX() + - drivetrain.getPose().getX(), + Field2d.getInstance().getAllianceSpeakerCenter().getY() + - drivetrain.getPose().getY(), + new Rotation2d()); + return new Rotation2d(Math.atan2(translation.getY(), translation.getX())); + })); + // field-relative toggle oi.getFieldRelativeButton() .toggleOnTrue( @@ -600,6 +618,7 @@ public void checkAllianceColor() { if (alliance.isPresent() && alliance.get() != lastAlliance) { this.lastAlliance = alliance.get(); this.drivetrain.updateAlliance(this.lastAlliance); + Field2d.getInstance().updateAlliance(this.lastAlliance); } } } diff --git a/src/main/java/frc/robot/configs/PracticeRobotConfig.java b/src/main/java/frc/robot/configs/PracticeRobotConfig.java index 62f45882..ac90053d 100644 --- a/src/main/java/frc/robot/configs/PracticeRobotConfig.java +++ b/src/main/java/frc/robot/configs/PracticeRobotConfig.java @@ -31,7 +31,7 @@ public class PracticeRobotConfig extends RobotConfig { private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10; private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9; private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11; - private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = -0.024658; + private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = 0.117188; private static final int GYRO_ID = 3; @@ -81,6 +81,7 @@ public class PracticeRobotConfig extends RobotConfig { Units.inchesToMeters(10.329), Units.inchesToMeters(18.387)), new Rotation3d(0, Units.degreesToRadians(-35), Units.degreesToRadians(90))); + // pitch 45 degrees // left camera private static final Transform3d ROBOT_TO_CAMERA_1 = diff --git a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java index 47fc338f..409a99dc 100644 --- a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java +++ b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java @@ -56,6 +56,11 @@ public Trigger getResetGyroButton() { @Override public Trigger getLock180Button() { + return new Trigger(() -> false); + } + + @Override + public Trigger getLockToSpeakerButton() { return rotateJoystickButtons[2]; } diff --git a/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java index 058734d1..6ff74e12 100644 --- a/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java +++ b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java @@ -59,7 +59,12 @@ public double getTranslateY() { @Override public Trigger getLock180Button() { - return translateJoystickButtons[3]; + return translateJoystickButtons[2]; + } + + @Override + public Trigger getLockToSpeakerButton() { + return rotateJoystickButtons[3]; } @Override diff --git a/src/main/java/frc/robot/operator_interface/OperatorInterface.java b/src/main/java/frc/robot/operator_interface/OperatorInterface.java index e5b69b64..835a1d57 100644 --- a/src/main/java/frc/robot/operator_interface/OperatorInterface.java +++ b/src/main/java/frc/robot/operator_interface/OperatorInterface.java @@ -64,4 +64,8 @@ public default Trigger getTurboButton() { public default Trigger getInterruptAll() { return new Trigger(() -> false); } + + public default Trigger getLockToSpeakerButton() { + return new Trigger(() -> false); + } } diff --git a/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java b/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java index d3a71345..da4798f8 100644 --- a/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java +++ b/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java @@ -60,4 +60,9 @@ public Trigger getVisionIsEnabledSwitch() { // vision is always enabled with Xbox as there is no switch to disable return new Trigger(() -> true); } + + @Override + public Trigger getLock180Button() { + return new Trigger(controller::getAButton); + } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index bdbd93d2..f4612137 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.2", + "version": "3.1.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.2" + "version": "3.1.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.2" + "version": "3.1.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.2" + "version": "3.1.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.2", + "version": "3.1.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae13633..787450f4 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a40798..2b7d1720 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e7..86329e30 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.2", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8e1b780b..20d95984 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.2", + "version": "v2024.2.5", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.2", + "version": "v2024.2.5", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.2", + "version": "v2024.2.5", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.2" + "version": "v2024.2.5" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.2" + "version": "v2024.2.5" } ] } \ No newline at end of file