diff --git a/Operator_Controls_Map_README.md b/Operator_Controls_Map_README.md index 3fedb6a7..ea3c9c0b 100644 --- a/Operator_Controls_Map_README.md +++ b/Operator_Controls_Map_README.md @@ -1,20 +1,18 @@ -| RPi Pico Pin | GP2040-CE | Button | Used For | -| ------------ | --------- | ------ | --------------------- | +| RPi Pico Pin | GP2040-CE | Button | Used For | +|-----------------|-----------|-----------|---------------------------------| | **MODE SWITCH** | -| GP16 | S1 | BACK | Want cube(T)/cone(F)) | -| **MANUAL** | -| GP2 | UP | POV UP | Arm manual up | -| GP3 | DOWN | POV DOWN | Arm manual down | -| GP4 | RIGHT | POV RIGHT | Arm manual extend | -| GP5 | LEFT | POV LEFT | Arm manual retrack | -| **SCORE** | -| GP6 | B1 | A | Arm score low node | -| GP7 | B2 | B | Arm score mid-node | -| GP11 | B4 | Y | Arm score high node | -| **UTILITY** | -| GP10 | B3 | X | Arm home | -| GP18 | L3 | LS | Arm stop | -| GP19 | R3 | RS | Arm stop | -| **ACQUIRE** | -| GP13 | L1 | LB | Arm to shelf | -| GP12 | R1 | RB | Arm floor | +| GP16 | S1 | BACK | Solenoid Locked(f)/Unlocked(t) | +| **ARM** | +| GP2 | UP | POV UP | Arm manual up | +| GP3 | DOWN | POV DOWN | Arm manual down | +| **CLIMBER** | +| GP4 | RIGHT | POV RIGHT | Climber manual up | +| GP5 | LEFT | POV LEFT | Climber manual down | +| **SCORE** | +| GP6 | B1 | A | Spin Up Flywheel | +| GP7 | B2 | B | Expel intake and indexer | +| GP11 | B4 | Y | Spin Flywheel Backwards | +| **UTILITY** | +| GP10 | B3 | X | Arm home | +| GP13 | L1 | LB | Arm align to podium (hopefully) | +| GP12 | R1 | RB | Duck | diff --git a/src/main/java/frc/lib/io/vision/Vision.java b/src/main/java/frc/lib/io/vision/Vision.java index 1e109765..0d64cf15 100644 --- a/src/main/java/frc/lib/io/vision/Vision.java +++ b/src/main/java/frc/lib/io/vision/Vision.java @@ -10,11 +10,11 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.utils.RobotOdometry; import frc.lib.utils.TunableNumber; import frc.robot.FieldConstants; -import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.Logger; @@ -38,7 +38,7 @@ 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 stdDevSlope = new TunableNumber("Vision/stdDevSlope", 0.50); private final TunableNumber stdDevPower = new TunableNumber("Vision/stdDevPower", 2.0); private final TunableNumber stdDevMultiTagFactor = new TunableNumber("Vision/stdDevMultiTagFactor", 0.2); @@ -98,16 +98,16 @@ public void periodic() { Pose2d estimatedRobotPose2d = ios[i].estimatedRobotPose.toPose2d(); // only update the pose estimator if the vision subsystem is enabled - if (isEnabled) { + if (isEnabled && !DriverStation.isAutonomous()) { // 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) - odometry.addVisionData( - List.of( - new RobotOdometry.TimestampedVisionUpdate( - ios[i].estimatedRobotPoseTimestamp, - estimatedRobotPose2d, - getStandardDeviations(i, estimatedRobotPose2d)))); + odometry + .getPoseEstimator() + .addVisionMeasurement( + estimatedRobotPose2d, + ios[i].estimatedRobotPoseTimestamp, + getStandardDeviations(i, estimatedRobotPose2d)); isVisionUpdating = true; } @@ -171,7 +171,12 @@ public void enable(boolean enable) { public boolean posesHaveConverged() { for (int i = 0; i < visionIOs.length; i++) { Pose3d robotPose = ios[i].estimatedRobotPose; - if (odometry.getLatestPose().minus(robotPose.toPose2d()).getTranslation().getNorm() + if (odometry + .getPoseEstimator() + .getEstimatedPosition() + .minus(robotPose.toPose2d()) + .getTranslation() + .getNorm() < poseDifferenceThreshold.get()) { Logger.recordOutput("Vision/posesInLine", true); return true; @@ -188,7 +193,7 @@ public boolean posesHaveConverged() { * @param estimatedPose The estimated pose to guess standard deviations for. */ private Matrix getStandardDeviations(int index, Pose2d estimatedPose) { - Matrix estStdDevs = VecBuilder.fill(0.1, 0.1, 1); + Matrix estStdDevs = VecBuilder.fill(1, 1, 0.5); int[] tags = ios[index].estimatedRobotPoseTags; int numTags = 0; double avgDist = 0; diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java index 6c16a38a..621057ef 100644 --- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java +++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java @@ -18,6 +18,15 @@ private AllianceFlipUtil() { throw new IllegalStateException("Utility class"); } + /** Negates a distance based on current alliance color. */ + public static double applyRelative(double distance) { + if (shouldFlip()) { + return -distance; + } else { + return distance; + } + } + /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ public static double apply(double xCoordinate) { if (shouldFlip()) { diff --git a/src/main/java/frc/lib/utils/RobotOdometry.java b/src/main/java/frc/lib/utils/RobotOdometry.java index c4ca27ba..10c3274e 100644 --- a/src/main/java/frc/lib/utils/RobotOdometry.java +++ b/src/main/java/frc/lib/utils/RobotOdometry.java @@ -1,182 +1,37 @@ package frc.lib.utils; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.List; -import java.util.NavigableMap; -import java.util.TreeMap; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import frc.robot.subsystems.drive.Drive; public class RobotOdometry { - private static final double historyLengthSecs = 0.3; - - private Pose2d basePose = new Pose2d(); - private Pose2d latestPose = new Pose2d(); - private final NavigableMap updates = new TreeMap<>(); - private final Matrix q = new Matrix<>(Nat.N3(), Nat.N1()); - - private static RobotOdometry instance = null; - - /** - * Initializes the RobotOdometry instance with the provided state standard deviations. - * - * @param stateStdDevs a Matrix representing the standard deviations of the robot state in terms - * of the x, y, and theta coordinates - */ - public RobotOdometry(Matrix stateStdDevs) { - for (int i = 0; i < 3; ++i) { - q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); - } + private static final RobotOdometry robotOdometry = new RobotOdometry(); + private SwerveDrivePoseEstimator estimator; + private SwerveModulePosition[] defaultPositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + private RobotOdometry() { + estimator = + new SwerveDrivePoseEstimator( + new SwerveDriveKinematics(Drive.getModuleTranslations()), + new Rotation2d(), + defaultPositions, + new Pose2d()); } public static RobotOdometry getInstance() { - if (instance == null) { - instance = new RobotOdometry(VecBuilder.fill(0.003, 0.003, 0.0002)); - } - return instance; - } - - /** Returns the latest robot pose based on drive and vision data. */ - public Pose2d getLatestPose() { - return latestPose; - } - - /** Resets the odometry to a known pose. */ - public void resetPose(Pose2d pose) { - basePose = pose; - updates.clear(); - update(); + return robotOdometry; } - /** Records a new drive movement. */ - public void addDriveData(double timestamp, Twist2d twist) { - updates.put(timestamp, new PoseUpdate(twist, new ArrayList<>())); - update(); - } - - /** Records a new set of vision updates. */ - public void addVisionData(List visionData) { - for (var timestampedVisionUpdate : visionData) { - var timestamp = timestampedVisionUpdate.timestamp(); - var visionUpdate = - new VisionUpdate(timestampedVisionUpdate.pose(), timestampedVisionUpdate.stdDevs()); - - if (updates.containsKey(timestamp)) { - // There was already an update at this timestamp, add to it - var oldVisionUpdates = updates.get(timestamp).visionUpdates(); - oldVisionUpdates.add(visionUpdate); - oldVisionUpdates.sort(VisionUpdate.compareDescStdDev); - - } else { - // Insert a new update - var prevUpdate = updates.floorEntry(timestamp); - var nextUpdate = updates.ceilingEntry(timestamp); - if (prevUpdate == null || nextUpdate == null) { - // Outside the range of existing data - return; - } - - // Create partial twists (prev -> vision, vision -> next) - var twist0 = - GeomUtils.multiplyTwist( - nextUpdate.getValue().twist(), - (timestamp - prevUpdate.getKey()) / (nextUpdate.getKey() - prevUpdate.getKey())); - var twist1 = - GeomUtils.multiplyTwist( - nextUpdate.getValue().twist(), - (nextUpdate.getKey() - timestamp) / (nextUpdate.getKey() - prevUpdate.getKey())); - - // Add new pose updates - var newVisionUpdates = new ArrayList(); - newVisionUpdates.add(visionUpdate); - newVisionUpdates.sort(VisionUpdate.compareDescStdDev); - updates.put(timestamp, new PoseUpdate(twist0, newVisionUpdates)); - updates.put( - nextUpdate.getKey(), new PoseUpdate(twist1, nextUpdate.getValue().visionUpdates())); - } - } - - // Recalculate latest pose once - update(); + public SwerveDrivePoseEstimator getPoseEstimator() { + return estimator; } - - /** Clears old data and calculates the latest pose. */ - private void update() { - // Clear old data and update base pose - while (updates.size() > 1 - && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { - var update = updates.pollFirstEntry(); - basePose = update.getValue().apply(basePose, q); - } - - // Update latest pose - latestPose = basePose; - for (var updateEntry : updates.entrySet()) { - latestPose = updateEntry.getValue().apply(latestPose, q); - } - } - - /** - * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of - * vision updates. - */ - private record PoseUpdate(Twist2d twist, ArrayList visionUpdates) { - public Pose2d apply(Pose2d lastPose, Matrix q) { - // Apply drive twist - var pose = lastPose.exp(twist); - - // Apply vision updates - for (var visionUpdate : visionUpdates) { - // Calculate Kalman gains based on std devs - // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) - Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); - var r = new double[3]; - for (int i = 0; i < 3; ++i) { - r[i] = visionUpdate.stdDevs().get(i, 0) * visionUpdate.stdDevs().get(i, 0); - } - for (int row = 0; row < 3; ++row) { - if (q.get(row, 0) == 0.0) { - visionK.set(row, row, 0.0); - } else { - visionK.set( - row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); - } - } - - // Calculate twist between current and vision pose - var visionTwist = pose.log(visionUpdate.pose()); - - // Multiply by Kalman gain matrix - var twistMatrix = - visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); - - // Apply twist - pose = - pose.exp( - new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); - } - - return pose; - } - } - - /** Represents a single vision pose with associated standard deviations. */ - public record VisionUpdate(Pose2d pose, Matrix stdDevs) { - public static final Comparator compareDescStdDev = - (VisionUpdate a, VisionUpdate b) -> { - return -Double.compare( - a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), - b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); - }; - } - - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - public record TimestampedVisionUpdate(double timestamp, Pose2d pose, Matrix stdDevs) {} } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64b30c92..a3caad3e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,7 +82,9 @@ public static Mode getMode() { } public static final Map logFolders = - Map.of(RobotType.ROBOT_2023, "/media/sda1", RobotType.ROBOT_2024_COMP, "/media/sda1"); + Map.of( + RobotType.ROBOT_2023, "/media/sda1", + RobotType.ROBOT_2024_COMP, "/media/sda1"); public enum RobotType { ROBOT_2023, diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 6cf64cc1..a2111c8b 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -84,21 +84,14 @@ public static final class Speaker { public static final class Subwoofer { public static final Pose2d ampFaceCorner = new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(239.366), - Rotation2d.fromDegrees(-120)); + Units.inchesToMeters(26.9), Units.inchesToMeters(260.15), Rotation2d.fromDegrees(64.6)); public static final Pose2d sourceFaceCorner = new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(197.466), - Rotation2d.fromDegrees(120)); + Units.inchesToMeters(24.2), Units.inchesToMeters(174.0), Rotation2d.fromDegrees(-61.4)); public static final Pose2d centerFace = - new Pose2d( - Units.inchesToMeters(35.775), - Units.inchesToMeters(218.416), - Rotation2d.fromDegrees(180)); + new Pose2d(Units.inchesToMeters(49.775), Units.inchesToMeters(218.416), new Rotation2d()); } public static final class Stage { diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 942201bf..222775cf 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -21,7 +21,6 @@ import frc.robot.subsystems.shooter.ShooterConstants; import java.util.Set; import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; public class Orchestrator { private final Drive drive; @@ -31,7 +30,6 @@ public class Orchestrator { private final Pixy2 pixy2; private final Arm arm; - @AutoLogOutput private boolean pullBack = false; private final Translation2d speaker = AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d()); @@ -65,7 +63,20 @@ public Command driveToNote(Supplier targetTranslation) { Supplier targetRotation = () -> targetTranslation.get().minus(drive.getPose().getTranslation()).getAngle(); return deferredStraightDriveToPose( - () -> new Pose2d(targetTranslation.get(), targetRotation.get())); + () -> + new Pose2d( + drive + .getPose() + .getTranslation() + .plus( + new Translation2d( + AllianceFlipUtil.applyRelative(Units.feetToMeters(0.531)), 0)), + targetRotation.get())) + .withTimeout(2) + .andThen( + deferredStraightDriveToPose( + () -> new Pose2d(targetTranslation.get(), drive.getRotation())) + .withTimeout(5)); } /** @@ -111,6 +122,18 @@ public Command alignArmAmp() { Commands.waitSeconds(3)); } + public Command duck() { + return arm.toSetpoint(ArmConstants.STOW.minus(Rotation2d.fromDegrees(5))) + .alongWith(Commands.runOnce(() -> RobotState.getInstance().duck = true)); + } + + public Command unDuck() { + return Commands.parallel( + arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS), Commands.waitUntil(arm::atSetpoint)) + .withTimeout(2) + .beforeStarting(() -> RobotState.getInstance().duck = false); + } + public Command goToAmp() { TunableNumber AMP_DISTANCE_OFFSET = new TunableNumber("Orchestrator/AmpDistance", 1); Supplier targetPose = @@ -162,7 +185,7 @@ public Command shootBasic() { return Commands.sequence( shooter .manualShoot(ShooterConstants.SHOOT_SPEED) - .withTimeout(1.5) + .withTimeout(.5) .until(() -> shooter.atVelocity(ShooterConstants.SHOOT_SPEED)) .andThen( Commands.race( @@ -191,6 +214,10 @@ public Command indexBasic() { .withTimeout(1); } + public Command stopFlywheel() { + return shooter.manualShoot(0).withTimeout(1); + } + /** * Calculates the angle to align the arm to the speaker from anywhere on the field. Does arcTan of * ((height of center of the speaker - height of the shooter) / distance to speaker) @@ -198,18 +225,38 @@ public Command indexBasic() { * @return The command to move the arm to the correct setPoint for shooting from its current * location. */ - public Command alignArmSpeaker() { // TODO: Not working. Abishek, Fix this + public Command alignArmSpeaker(Supplier robotPose) { return Commands.defer( - () -> - arm.toSetpoint( - new Rotation2d( - Math.abs( - Math.atan( - (FieldConstants.Speaker.centerSpeakerOpening.getZ() - - Math.sin(arm.getAngle() + ArmConstants.STOW.getRadians()) - * Units.inchesToMeters(28)) - / drive.getPose().getTranslation().getDistance(speaker))))), + () -> { + Supplier distance = + () -> + robotPose + .get() + .getTranslation() + .getDistance( + AllianceFlipUtil.apply( + FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d())); + return arm.toSetpoint( + () -> + Rotation2d.fromDegrees( + (-3.1419 * (distance.get() * distance.get())) + + (23.725 * distance.get()) + - 30.103)); + }, Set.of(arm)); + // return Commands.defer( + // () -> + // arm.toSetpoint( + // new Rotation2d( + // Math.abs( + // Math.atan( + // (FieldConstants.Speaker.centerSpeakerOpening.getZ() + // - Math.sin(arm.getAngle() + + // ArmConstants.STOW.getRadians()) + // * Units.inchesToMeters(28)) + // / drive.getPose().getTranslation().getDistance(speaker))))), + // Set.of(arm)); + // return Commands.none(); } /** @@ -217,8 +264,8 @@ public Command alignArmSpeaker() { // TODO: Not working. Abishek, Fix this * * @return The command to move the robot and the arm in preparation to shoot. */ - public Command fullAlignSpeaker() { - return Commands.sequence(turnToSpeaker(), alignArmSpeaker()); + public Command fullAlignSpeaker(Supplier robotPose) { + return Commands.sequence(turnToSpeaker(), alignArmSpeaker(robotPose)); } /** @@ -227,8 +274,8 @@ public Command fullAlignSpeaker() { * * @return The command to align both the robot and the arm, and then shoots at full power. */ - public Command fullAlignShootSpeaker() { - return Commands.sequence(fullAlignSpeaker(), shootBasic()); + public Command fullAlignShootSpeaker(Supplier robotPose) { + return Commands.sequence(fullAlignSpeaker(robotPose), shootBasic()); } /** @@ -244,26 +291,24 @@ public Command intakeBasic() { Commands.parallel( indexer.setPercent(IndexerConstants.INDEX_SPEED.get()), intake.intake()) .until(() -> RobotState.getInstance().hasNote) - .withTimeout(10) - .finallyDo(() -> pullBack = false)) - .andThen(pullBack().finallyDo(() -> pullBack = true)); + .withTimeout(10)) + .andThen(pullBack()); } public Command pullBack() { - return (Commands.parallel( - indexer.setPercent(IndexerConstants.BACKUP_SPEED), - shooter.manualShoot(-0.2), - intake.stop()) - .withTimeout(IndexerConstants.BACKUP_TIME) - .andThen( - Commands.parallel( - arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS).until(arm::atSetpoint), - indexer.setPercent(0).until(() -> true), - shooter.manualShoot(0).until(() -> true), - intake.stop().until(() -> true)) - .withTimeout(5)) - .finallyDo(() -> pullBack = true)) - .onlyIf(() -> !pullBack); + return Commands.parallel( + indexer.setPercent(IndexerConstants.BACKUP_SPEED), + shooter.manualShoot(-0.2), + intake.stop()) + .withTimeout(IndexerConstants.BACKUP_TIME) + .andThen( + Commands.parallel( + arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS).until(arm::atSetpoint), + indexer.setPercent(0).until(() -> true), + shooter.manualShoot(0).until(() -> true), + intake.stop().until(() -> true), + Commands.waitSeconds(0.5)) + .withTimeout(5)); } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 053b7d9e..3c102581 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.RobotController; @@ -30,6 +31,7 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private RobotState state = RobotState.getInstance(); private PowerDistribution pdp; + private LinearFilter batteryFilter = LinearFilter.movingAverage(15); private static final int LOW_VOLTAGE = 9; @@ -111,7 +113,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); // If robot has low battery, lowbatteryalert will be set to true - state.lowBatteryAlert = RobotController.getBatteryVoltage() < LOW_VOLTAGE; + state.lowBatteryAlert = + batteryFilter.calculate(RobotController.getBatteryVoltage()) < LOW_VOLTAGE; } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7bf6a738..75967222 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,10 +17,12 @@ import frc.lib.io.gyro3d.GyroIO; import frc.lib.io.gyro3d.GyroPigeon2; import frc.lib.io.vision.Vision; +import frc.lib.io.vision.VisionIOPhotonVision; import frc.lib.utils.AllianceFlipUtil; import frc.robot.commands.auto.Autos; import frc.robot.commands.drive.DriveWithDpad; import frc.robot.commands.drive.DriveWithJoysticks; +import frc.robot.commands.drive.StolenJoystick; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.arm.ArmIO; @@ -28,6 +30,7 @@ import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberConstants; import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberIOSparkMax; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; @@ -44,6 +47,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOPhysical; +import java.util.List; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -105,12 +109,12 @@ public RobotContainer() { Units.inchesToMeters(15.5)), new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(180))); - // vision = - // new Vision( - // List.of( - // new VisionIOPhotonVision("front", front), - // new VisionIOPhotonVision("back", back)) - // .toArray(new frc.lib.io.vision.VisionIO[0])); + vision = + new Vision( + List.of( + new VisionIOPhotonVision("front", front), + new VisionIOPhotonVision("back", back)) + .toArray(new frc.lib.io.vision.VisionIO[0])); drive = new Drive( @@ -124,7 +128,8 @@ public RobotContainer() { intake = new Intake(new IntakeIOPhysical()); shooter = new Shooter(new ShooterIOPhysical()); leds = new Leds(); - // climber = new Climber(new ClimberIOSparkMax()); + // pixy2 = new Pixy2(new Pixy2IOPhysical()); + climber = new Climber(new ClimberIOSparkMax()); } case ROBOT_SIMBOT -> { @@ -192,7 +197,8 @@ public RobotContainer() { autoChooser.addOption( "Score One Note + Mobility [LEFT]", autos.scoreOneNoteMobility(Autos.StartingPosition.LEFT)); - autoChooser.addOption("Score Two Notes [LEFT]", autos.twoNoteAuto(Autos.StartingPosition.LEFT)); + autoChooser.addOption( + "Score Two Notes [LEFT]", autos.noVisionTwoNoteAuto(Autos.StartingPosition.LEFT)); autoChooser.addOption( "Score Three Notes [LEFT]", autos.threeNoteAuto(Autos.StartingPosition.LEFT)); autoChooser.addOption("Mobility [RIGHT]", autos.mobilityAuto(Autos.StartingPosition.RIGHT)); @@ -200,7 +206,7 @@ public RobotContainer() { "Score One Note + Mobility [RIGHT]", autos.scoreOneNoteMobility(Autos.StartingPosition.RIGHT)); autoChooser.addOption( - "Score Two Notes [RIGHT]", autos.twoNoteAuto(Autos.StartingPosition.RIGHT)); + "Score Two Notes [RIGHT]", autos.noVisionTwoNoteAuto(Autos.StartingPosition.RIGHT)); autoChooser.addOption( "Score Three Notes [RIGHT]", autos.threeNoteAuto(Autos.StartingPosition.RIGHT)); autoChooser.addOption("Mobility [CENTER]", autos.mobilityAuto(Autos.StartingPosition.CENTER)); @@ -208,9 +214,13 @@ public RobotContainer() { "Score One Note + Mobility [CENTER]", autos.scoreOneNoteMobility(Autos.StartingPosition.CENTER)); autoChooser.addOption( - "Score Two Notes [CENTER]", autos.twoNoteAuto(Autos.StartingPosition.CENTER)); + "Score Two Notes [CENTER]", autos.noVisionTwoNoteAuto(Autos.StartingPosition.CENTER)); + autoChooser.addOption( + "Score Three Notes (amp) [CENTER]", autos.threeNoteAuto(Autos.StartingPosition.CENTER)); autoChooser.addOption( - "Score Three Notes [CENTER]", autos.threeNoteAuto(Autos.StartingPosition.CENTER)); + "Score Three Notes (stage) [CENTER]", + autos.threeNoteStageAuto(Autos.StartingPosition.CENTER)); + autoChooser.addOption("Score Four Notes [CENTER]", autos.noVisionFourNoteAuto()); // Rumble on intake new Trigger(() -> RobotState.getInstance().hasNote) @@ -255,7 +265,6 @@ private void configureButtonBindings() { driverController .pov(-1) .whileFalse(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - // stop when doing nothing intake.setDefaultCommand(intake.stop()); indexer.setDefaultCommand(indexer.setPercent(0)); @@ -273,16 +282,8 @@ private void configureButtonBindings() { // Hold Y: Expel the shooter operatorController.y().whileTrue(shooter.manualShoot(-1)); // Hold RB: Duck the arm to fit under stage - operatorController - .rightBumper() - .whileTrue(arm.toSetpoint(ArmConstants.STOW.minus(Rotation2d.fromDegrees(5)))); - operatorController - .rightBumper() - .onFalse( - Commands.parallel( - arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS), - Commands.waitUntil(arm::atSetpoint)) - .withTimeout(2)); + operatorController.rightBumper().whileTrue(orchestrator.duck()); + operatorController.rightBumper().onFalse(orchestrator.unDuck()); // Back button (toggle switch): unlock/lock climber ratchet operatorController.back().whileTrue(climber.setRatchet(false)); @@ -308,13 +309,27 @@ private void configureButtonBindings() { // driver controller // Click Right Bumper: Move arm to stow position + // driverController + // .rightBumper() + // .onTrue( + // Commands.parallel( + // arm.toSetpoint(ArmConstants.STOW.minus(Rotation2d.fromDegrees(5))), + // Commands.waitUntil(arm::limitSwitchPressed)) + // .withTimeout(2)); + // driverController + // .rightBumper() + // .whileTrue( driverController .rightBumper() - .onTrue( - Commands.parallel( - arm.toSetpoint(ArmConstants.STOW.minus(Rotation2d.fromDegrees(5))), - Commands.waitUntil(arm::limitSwitchPressed)) - .withTimeout(2)); + .whileTrue( + new StolenJoystick( + drive, + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> drive.getPose(), + FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d(), + () -> true) + .alongWith(orchestrator.alignArmSpeaker(() -> drive.getPose()))); // Click Left Bumper: Move arm to amp position driverController.leftBumper().onTrue(orchestrator.alignArmAmp()); // Click left Trigger: Intake (until clicked again or has a note) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index c6ebe181..58fa1fe4 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -29,8 +29,14 @@ public class RobotState { public boolean intaking = false; /** Set true if the robot is currently hanging from the chain or in the process of climbing. */ - @AutoLogOutput(key = "RobotState/Hanging") - public boolean hanging = false; + @AutoLogOutput(key = "RobotState/ClimberUp") + public boolean climberUp = false; + + @AutoLogOutput(key = "RobotState/ClimberDown") + public boolean climberDown = false; + + @AutoLogOutput(key = "RobotState/ClimberRatchet") + public boolean climberRatchet = true; /** * Set true if the robot currently contains a note. Used to prevent the robot from picking up a @@ -47,6 +53,14 @@ public class RobotState { @AutoLogOutput(key = "RobotState/NoteAngle") public double noteAngle = 0; + /** If the duck button is pressed */ + @AutoLogOutput(key = "RobotState/Duck") + public boolean duck = false; + + /** If the shooter is at the speed needed to shoot into the speaker */ + @AutoLogOutput(key = "RobotState/ShooterSpeedReady") + public boolean shooterSpeedIsReady = false; + /** The singleton instance of the RobotState class. */ private static RobotState instance = null; diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 4236ac00..3b57a311 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -14,23 +14,19 @@ import frc.robot.subsystems.arm.ArmConstants; import frc.robot.subsystems.drive.Drive; import java.util.Map; +import java.util.Set; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; public class Autos { private final Drive drive; private final Arm arm; private final Orchestrator orchestrator; - @AutoLogOutput(key = "Autos/Notes/0") - private Translation2d noteTranslation; + private double MOBILITY_DRIVE_DISTANCE = Units.feetToMeters(6.75); - @AutoLogOutput(key = "Autos/Notes/1") - private Translation2d secondNoteTranslation; - - @AutoLogOutput(key = "Autos/Notes/2") - private Translation2d thirdNoteTranslation; + // @AutoLogOutput(key = "Autos/Notes/") + private Translation2d[] noteTranslations = new Translation2d[3]; public Autos(Drive drive, Arm arm, Orchestrator orchestrator) { this.drive = drive; @@ -39,198 +35,305 @@ public Autos(Drive drive, Arm arm, Orchestrator orchestrator) { } public enum StartingPosition { - LEFT( - new Pose2d( - AllianceFlipUtil.apply( - AllianceFlipUtil.shouldFlip() - ? FieldConstants.Subwoofer.sourceFaceCorner - : FieldConstants.Subwoofer.ampFaceCorner) - .getTranslation(), - AllianceFlipUtil.apply( - AllianceFlipUtil.shouldFlip() - ? FieldConstants.Subwoofer.sourceFaceCorner - : FieldConstants.Subwoofer.ampFaceCorner) - .getRotation() - .plus(Rotation2d.fromDegrees(180)))), - CENTER( - new Pose2d( - AllianceFlipUtil.apply(FieldConstants.Subwoofer.centerFace.getTranslation()), - AllianceFlipUtil.apply( - FieldConstants.Subwoofer.centerFace - .getRotation() - .plus(Rotation2d.fromRadians(Units.degreesToRadians(180)))))), - RIGHT( - new Pose2d( - AllianceFlipUtil.apply( - AllianceFlipUtil.shouldFlip() - ? FieldConstants.Subwoofer.ampFaceCorner - : FieldConstants.Subwoofer.sourceFaceCorner) - .getTranslation(), - AllianceFlipUtil.apply( - AllianceFlipUtil.shouldFlip() - ? FieldConstants.Subwoofer.ampFaceCorner - : FieldConstants.Subwoofer.sourceFaceCorner) - .getRotation() - .plus(Rotation2d.fromDegrees(180)))); - private final Pose2d startingPosition; - - StartingPosition(Pose2d startingPosition) { - this.startingPosition = startingPosition; - Logger.recordOutput("Autos/StartingPositions/" + this, startingPosition); + LEFT, + CENTER, + RIGHT; + + public Pose2d getStartingPosition() { + return getRelativeStartingPosition().getStartingPosition(); + } + + RelativeStartingPosition getRelativeStartingPosition() { + switch (this) { + case LEFT: + return AllianceFlipUtil.shouldFlip() + ? RelativeStartingPosition.NEAR_SOURCE + : RelativeStartingPosition.NEAR_AMP; + case CENTER: + return RelativeStartingPosition.CENTER; + + case RIGHT: + return AllianceFlipUtil.shouldFlip() + ? RelativeStartingPosition.NEAR_AMP + : RelativeStartingPosition.NEAR_SOURCE; + } + throw new RuntimeException("Invalid position"); } + } + + private enum RelativeStartingPosition { + NEAR_AMP, + CENTER, + NEAR_SOURCE; public Pose2d getStartingPosition() { - return startingPosition; + switch (this) { + case NEAR_AMP: + return AllianceFlipUtil.apply(FieldConstants.Subwoofer.ampFaceCorner); + case CENTER: + return AllianceFlipUtil.apply(FieldConstants.Subwoofer.centerFace); + case NEAR_SOURCE: + return AllianceFlipUtil.apply(FieldConstants.Subwoofer.sourceFaceCorner); + } + throw new RuntimeException("Invalid position"); } } private Command setNotePositions(Supplier position) { return Commands.runOnce( - () -> { - switch (position.get()) { - case RIGHT -> { - this.noteTranslation = getNotePositions(0, false); - this.secondNoteTranslation = getNotePositions(1, false); - this.thirdNoteTranslation = getNotePositions(2, true); - } - case CENTER -> { - this.noteTranslation = getNotePositions(1, false); - this.secondNoteTranslation = getNotePositions(2, false); - this.thirdNoteTranslation = getNotePositions(0, true); - } - case LEFT -> { - this.noteTranslation = getNotePositions(2, false); - this.secondNoteTranslation = getNotePositions(1, false); - this.thirdNoteTranslation = getNotePositions(0, true); - } - } - }); - } - - private Translation2d getNotePositions(int index, boolean centerNotes) { - return AllianceFlipUtil.apply( + () -> { + switch (position.get()) { + case RIGHT -> { + this.noteTranslations[0] = getNotePositions(0, false, true); + this.noteTranslations[1] = getNotePositions(1, false, false); + this.noteTranslations[2] = getNotePositions(2, false, false); + } + case CENTER -> { + this.noteTranslations[0] = getNotePositions(1, false, true, false); + this.noteTranslations[1] = getNotePositions(2, false, false, false); + this.noteTranslations[2] = getNotePositions(0, false, false, false); + } + case LEFT -> { + this.noteTranslations[0] = getNotePositions(2, false, true); + this.noteTranslations[1] = getNotePositions(1, false, false); + this.noteTranslations[2] = getNotePositions(0, false, false); + } + } + }) + .until(() -> true); + } + + private Translation2d getNotePositions(int index, boolean centerNotes, boolean offset) { + return getNotePositions(index, centerNotes, offset, true); + } + + private Translation2d getNotePositions( + int index, boolean centerNotes, boolean offset, boolean flip) { + Translation2d noteTranslation = centerNotes ? FieldConstants.StagingLocations.centerlineTranslations[ - AllianceFlipUtil.shouldFlip() ? 4 - index : index] + flip && AllianceFlipUtil.shouldFlip() ? 4 - index : index] : FieldConstants.StagingLocations.spikeTranslations[ - AllianceFlipUtil.shouldFlip() ? 2 - index : index]); + flip && AllianceFlipUtil.shouldFlip() ? 2 - index : index]; + return AllianceFlipUtil.apply( + new Translation2d( + noteTranslation.getX() - (offset ? Units.inchesToMeters(7) : 0), + noteTranslation.getY())); } public Command mobilityAuto(StartingPosition position) { return Commands.runOnce(() -> drive.setPose(position.getStartingPosition())) .andThen( - Commands.either( - Commands.select( - Map.of( - StartingPosition.RIGHT, - new StraightDriveToPose(Units.feetToMeters(-6.75), 0, 0, drive) - .withTimeout(5), - StartingPosition.CENTER, - new StraightDriveToPose(Units.feetToMeters(-6.75), 0, 0, drive) - .withTimeout(5), - StartingPosition.LEFT, - Commands.run( - () -> - drive.runVelocity( - new ChassisSpeeds(Units.feetToMeters(9), 0, 0))) - .withTimeout(1) - .andThen( - new StraightDriveToPose(Units.feetToMeters(-6.75), 0, 0, drive) - .withTimeout(5))), - () -> position), - Commands.select( - Map.of( - StartingPosition.LEFT, - new StraightDriveToPose(Units.feetToMeters(6.75), 0, 0, drive) - .withTimeout(5), - StartingPosition.CENTER, - new StraightDriveToPose(Units.feetToMeters(6.75), 0, 0, drive) - .withTimeout(5), - StartingPosition.RIGHT, - Commands.run( - () -> - drive.runVelocity( - new ChassisSpeeds(Units.feetToMeters(9), 0, 0))) - .withTimeout(1) - .andThen( - new StraightDriveToPose(Units.feetToMeters(6.75), 0, 0, drive) - .withTimeout(5))), - () -> position), - AllianceFlipUtil::shouldFlip)); + Commands.select( + Map.of( + RelativeStartingPosition.NEAR_AMP, + driveXDistance(MOBILITY_DRIVE_DISTANCE).withTimeout(5), + RelativeStartingPosition.CENTER, + driveXDistance(MOBILITY_DRIVE_DISTANCE).withTimeout(5), + RelativeStartingPosition.NEAR_SOURCE, + Commands.run( + () -> drive.runVelocity(new ChassisSpeeds(Units.feetToMeters(9), 0, 0))) + .withTimeout(1) + .andThen(driveXDistance(MOBILITY_DRIVE_DISTANCE).withTimeout(5))), + position::getRelativeStartingPosition)); + } + + private Command driveXDistance(double distance) { + return Commands.defer( + () -> new StraightDriveToPose(AllianceFlipUtil.applyRelative(distance), 0, 0, drive), + Set.of(drive)); } public Command oneNoteAuto() { return arm.runPercent(-0.3) + .raceWith(orchestrator.spinUpFlywheel().withTimeout(1.7)) .until(arm::limitSwitchPressed) .withTimeout(1) - .andThen(arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS).withTimeout(1)) - .andThen(orchestrator.shootBasic()); + .andThen( + Commands.parallel( + arm.toSetpoint(ArmConstants.AFTER_INTAKE_POS).withTimeout(.5), + Commands.waitUntil(arm::atSetpoint).withTimeout(.2), + orchestrator.spinUpFlywheel().withTimeout(1))) + .andThen( + orchestrator.indexBasic().alongWith(orchestrator.spinUpFlywheel()).withTimeout(0.5)); } public Command scoreOneNoteMobility(StartingPosition position) { return oneNoteAuto().andThen(mobilityAuto(position)); } - public Command twoNoteAuto(StartingPosition position) { - return Commands.runOnce(() -> drive.setPose(position.getStartingPosition())) - .andThen(setNotePositions(() -> position)) + public Command noVisionFourNoteAuto() { + return noVisionInit(() -> StartingPosition.CENTER) .andThen( oneNoteAuto() .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> noteTranslation).withTimeout(5), - orchestrator.intakeBasic())) - .andThen(shoot())); + scoreCycle(() -> noteTranslations[0], Rotation2d.fromDegrees(5), () -> false)) + .andThen( + scoreCycle(() -> noteTranslations[1], Rotation2d.fromDegrees(10), () -> false)) + .andThen(stageNoteCycle(() -> noteTranslations[2], Rotation2d.fromDegrees(10)))); + } + + private Command noVisionInit(Supplier position) { + return Commands.parallel( + Commands.runOnce(() -> drive.setPose(position.get().getStartingPosition())) + .withTimeout(0.02), + setNotePositions(position).withTimeout(0.02)); } public Command threeNoteAuto(StartingPosition position) { - return setNotePositions(() -> position) + return noVisionInit(() -> position) + .andThen(oneNoteAuto()) .andThen( - oneNoteAuto() - .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> noteTranslation), - orchestrator.intakeBasic())) - .andThen(shoot()) - .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> secondNoteTranslation), - orchestrator.intakeBasic()) - .andThen(shoot()))); - } - - private Command shoot() { - return Commands.run(() -> drive.runVelocity(new ChassisSpeeds(0, 0, 0))) - .onlyWhile( - () -> - drive - .getPose() - .getTranslation() - .getDistance( - AllianceFlipUtil.apply( - FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d())) - > Units.inchesToMeters(30)) - .andThen(orchestrator.fullAlignShootSpeaker()); - } - - public Command fourNoteAuto(StartingPosition position) { - return setNotePositions(() -> position) + scoreCycle( + () -> noteTranslations[0], + position::getStartingPosition, + () -> position != StartingPosition.CENTER)) + .andThen(scoreCycle(() -> noteTranslations[1], position::getStartingPosition, () -> true)); + } + + public Command noVisionTwoNoteAuto(StartingPosition position) { + return noVisionInit(() -> position) + .andThen(oneNoteAuto()) .andThen( - oneNoteAuto() + scoreCycle( + () -> noteTranslations[0], + position::getStartingPosition, + () -> position != StartingPosition.CENTER)); + } + + public Command threeNoteStageAuto(StartingPosition position) { + return noVisionInit(() -> position) + .andThen(oneNoteAuto()) + .andThen( + scoreCycle( + () -> noteTranslations[0], + position::getStartingPosition, + () -> position != StartingPosition.CENTER)) + .andThen(stageNoteCycleSubwoofer(() -> noteTranslations[2], position::getStartingPosition)); + } + + private Command scoreCycle( + Supplier intakePosition, + Supplier shootPosition, + BooleanSupplier backUp) { + return orchestrator + .stopFlywheel() + .andThen( + Commands.parallel( + Commands.sequence( + backUp().onlyIf(backUp), orchestrator.driveToNote(intakePosition)), + orchestrator.intakeBasic()) + .withTimeout(3)) + .andThen( + orchestrator + .deferredStraightDriveToPose(shootPosition) + .withTimeout(2.5) + .alongWith(orchestrator.spinUpFlywheel().withTimeout(1.5))) + .andThen(orchestrator.indexBasic().alongWith(orchestrator.spinUpFlywheel()).withTimeout(1)); + } + + private Command backUp() { + return Commands.run(() -> drive.runVelocity(new ChassisSpeeds(Units.feetToMeters(4), 0, 0))) + .withTimeout(0.5); + } + + private Command scoreCycle( + Supplier intakePosition, Rotation2d armAngle, BooleanSupplier backUp) { + return orchestrator + .stopFlywheel() + .andThen( + Commands.parallel( + Commands.sequence( + backUp().onlyIf(backUp), + orchestrator.driveToNote(intakePosition).withTimeout(3)), + orchestrator.intakeBasic())) + .andThen(Commands.waitSeconds(0.75)) + .andThen( + Commands.parallel( + orchestrator.turnToSpeaker().withTimeout(1.5), + arm.toSetpoint(armAngle).withTimeout(0.2), + Commands.waitUntil(arm::atSetpoint).withTimeout(.2), + orchestrator.spinUpFlywheel().withTimeout(1.7)) .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> noteTranslation), - orchestrator.intakeBasic())) - .andThen(shoot()) + orchestrator + .indexBasic() + .alongWith(Commands.print("actually indexing")) + .withTimeout(1))) + .andThen(orchestrator.stopFlywheel()); + } + + private Command stageNoteCycle(Supplier intakePosition, Rotation2d armAngle) { + return Commands.race( + orchestrator + .deferredStraightDriveToPose( + () -> + new Pose2d( + drive.getPose().getTranslation().getX() + - AllianceFlipUtil.applyRelative(Units.inchesToMeters(20)), + intakePosition.get().getY() + + AllianceFlipUtil.applyRelative(Units.inchesToMeters(-5)), + new Rotation2d())) .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> secondNoteTranslation), - orchestrator.intakeBasic()) - .andThen(shoot())) + orchestrator.deferredStraightDriveToPose( + () -> + new Pose2d( + intakePosition.get().getX() + - AllianceFlipUtil.applyRelative(Units.inchesToMeters(9)), + drive.getPose().getTranslation().getY(), + new Rotation2d()))) + .withTimeout(3), + orchestrator.intakeBasic()) + .withTimeout(5) + .andThen( + Commands.parallel( + orchestrator.turnToSpeaker().withTimeout(2), + arm.toSetpoint(armAngle).withTimeout(0.2), + Commands.waitUntil(arm::atSetpoint).withTimeout(0.2), + Commands.waitSeconds(0.1), + orchestrator.spinUpFlywheel()) .andThen( - Commands.parallel( - orchestrator.driveToNote(() -> thirdNoteTranslation), - orchestrator.intakeBasic())) - .andThen(shoot())); + orchestrator + .indexBasic() + .alongWith(orchestrator.spinUpFlywheel()) + .withTimeout(0.5))) + .withTimeout(3.5); + } + + private Command stageNoteCycleSubwoofer( + Supplier intakePosition, Supplier shootPosition) { + return orchestrator + .stopFlywheel() + .andThen( + Commands.parallel( + orchestrator + .deferredStraightDriveToPose( + () -> + new Pose2d( + drive.getPose().getTranslation().getX() + + AllianceFlipUtil.applyRelative(Units.inchesToMeters(5)), + intakePosition.get().getY() + + AllianceFlipUtil.applyRelative(Units.inchesToMeters(-5)), + drive.getRotation())) + .andThen( + orchestrator.deferredStraightDriveToPose( + () -> + new Pose2d( + intakePosition.get().getX() + - AllianceFlipUtil.applyRelative(Units.inchesToMeters(15)), + drive.getPose().getTranslation().getY(), + intakePosition + .get() + .minus(drive.getPose().getTranslation()) + .getAngle()))) + .withTimeout(3), + orchestrator.intakeBasic())) + .withTimeout(5) + .andThen( + orchestrator + .deferredStraightDriveToPose(shootPosition) + .withTimeout(2.5) + .alongWith(orchestrator.spinUpFlywheel().withTimeout(1.5))) + .andThen(orchestrator.indexBasic().alongWith(orchestrator.spinUpFlywheel()).withTimeout(1)) + .andThen(orchestrator.stopFlywheel()); } } diff --git a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java index 7988ad2f..112c8e2b 100644 --- a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java @@ -29,13 +29,13 @@ public class StraightDriveToPose extends Command { new ProfiledPIDController( // 2.0, 0, 0.0, new Constraints(Units.inchesToMeters(150), // Units.inchesToMeters(450.0))); - 2.0, + 3.0, 0, 0.0, new Constraints(Units.inchesToMeters(85), Units.inchesToMeters(450.0))); // 90 private final ProfiledPIDController thetaController = - new ProfiledPIDController( - 5.0, 0, 0.0, new Constraints(Units.degreesToRadians(360), Units.degreesToRadians(720.0))); + new ProfiledPIDController( // TODO: Tune this + 3.0, 0, .01, new Constraints(Units.degreesToRadians(360), Units.degreesToRadians(720.0))); private Pose2d targetPose; private double driveErrorAbs; diff --git a/src/main/java/frc/robot/commands/drive/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/drive/DriveWithJoysticks.java index 2eef0e31..beb7a00d 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithJoysticks.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithJoysticks.java @@ -43,6 +43,11 @@ public DriveWithJoysticks( this.rightXSupplier = rightXSupplier; this.robotRelativeOverride = robotRelativeOverride; + /* + https://www.khanacademy.org/science/physics/torque-angular-momentum/rotational-kinematics/v/relationship-between-angular-velocity-and-speed + If speed = angular velocity * radius, then we can re-arrange it into Angular Velocity = Speed / Radius. + We assume that the speed we care about is the maximum speed, and our radius is distance from the center of the robot to a module (ie Translation2d::getNorm). + */ MAX_ANGULAR_SPEED = DriveConstants.MAX_LINEAR_SPEED / Arrays.stream(drive.getModuleTranslations()) @@ -103,8 +108,8 @@ public void execute() { ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * DriveConstants.MAX_LINEAR_SPEED, linearVelocity.getY() * DriveConstants.MAX_LINEAR_SPEED, - rightX * DriveConstants.MAX_LINEAR_SPEED, - RobotOdometry.getInstance().getLatestPose().getRotation()); + rightX * MAX_ANGULAR_SPEED, + RobotOdometry.getInstance().getPoseEstimator().getEstimatedPosition().getRotation()); } drive.runVelocity(speeds); } diff --git a/src/main/java/frc/robot/commands/drive/StolenJoystick.java b/src/main/java/frc/robot/commands/drive/StolenJoystick.java new file mode 100644 index 00000000..1730338e --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/StolenJoystick.java @@ -0,0 +1,137 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.utils.AllianceFlipUtil; +import frc.lib.utils.GeomUtils; +import frc.lib.utils.RobotOdometry; +import frc.lib.utils.TunableNumber; +import frc.robot.Constants; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; +import java.util.Arrays; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class StolenJoystick extends Command { + private final Drive drive; + private final Supplier leftXSupplier; + private final Supplier leftYSupplier; + private final Supplier robotOrientation; + private final Translation2d targetPosition; + private final Supplier robotRelativeOverride; + + private static final double TIME_TO_FULL_SPEED = 0.15; + private final SlewRateLimiter leftXFilter = new SlewRateLimiter(1 / TIME_TO_FULL_SPEED); + private final SlewRateLimiter leftYFilter = new SlewRateLimiter(1 / TIME_TO_FULL_SPEED); + + private TunableNumber rotationKP = new TunableNumber("StolenJoystick/KP", 2.5); + private TunableNumber rotationKD = new TunableNumber("StolenJoystick/KD", 0.0); + private final PIDController rotationPid; + private final double MAX_ANGULAR_SPEED; + + public StolenJoystick( + Drive drive, + Supplier leftXSupplier, + Supplier leftYSupplier, + Supplier robotOrientation, + Translation2d targetPosition, + Supplier robotRelativeOverride) { + this.drive = drive; + this.leftXSupplier = leftXSupplier; + this.leftYSupplier = leftYSupplier; + this.robotOrientation = robotOrientation; + this.targetPosition = targetPosition; + this.robotRelativeOverride = robotRelativeOverride; + + MAX_ANGULAR_SPEED = + DriveConstants.MAX_LINEAR_SPEED + / Arrays.stream(drive.getModuleTranslations()) + .map(Translation2d::getNorm) + .max(Double::compare) + .get(); + + rotationPid = new PIDController(rotationKP.get(), 0.0, rotationKD.get()); + rotationPid.enableContinuousInput(-Math.PI, Math.PI); + rotationPid.setTolerance(Units.degreesToRadians(4)); + + addRequirements(drive); + } + + @Override + public void execute() { + // Update controllers if tunable numbers have changed + if (Constants.tuningMode) { + if (rotationKP.hasChanged(hashCode()) || rotationKD.hasChanged(hashCode())) { + rotationPid.setPID(rotationKP.get(), 0, rotationKD.get()); + } + } + + // Get values from double suppliers + double leftX = leftXFilter.calculate(leftXSupplier.get()); + double leftY = leftYFilter.calculate(leftYSupplier.get()); + + // Get direction and magnitude of linear axes + double linearMagnitude = Math.hypot(leftX, leftY); + Rotation2d linearDirection = new Rotation2d(leftX, leftY); + + // Apply deadband + linearMagnitude = MathUtil.applyDeadband(linearMagnitude, 0.1); + + // Apply squaring + linearMagnitude = Math.copySign(linearMagnitude * linearMagnitude, linearMagnitude); + + // Apply speed limits //TODO: Custom limits + linearMagnitude *= 1.0; + + // Calcaulate new linear components + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(GeomUtils.transformFromTranslation(linearMagnitude, 0)) + .getTranslation(); + + // Calculate the target angle based on the current position of the robot and the speaker + Rotation2d targetAngle = + new Pose2d( + robotOrientation.get().getTranslation(), + AllianceFlipUtil.apply(targetPosition) + .minus(robotOrientation.get().getTranslation()) + .getAngle() + .minus(Rotation2d.fromDegrees(180))) + .getRotation(); + Logger.recordOutput("Drive/TargetAngle", targetAngle); + double angularVelocity = + rotationPid.calculate( + robotOrientation.get().getRotation().getRadians(), targetAngle.getRadians()); + // Convert to meters per second + ChassisSpeeds speeds; + Logger.recordOutput("StolenJoystick/AngularVelocity", angularVelocity); + + if (AllianceFlipUtil.shouldFlip()) { + Logger.recordOutput("Drive/FlipAlliance", true); + linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI)); + } else { + Logger.recordOutput("Drive/FlipAlliance", false); + } + speeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * DriveConstants.MAX_LINEAR_SPEED, + linearVelocity.getY() * DriveConstants.MAX_LINEAR_SPEED, + angularVelocity, + RobotOdometry.getInstance().getPoseEstimator().getEstimatedPosition().getRotation()); + + drive.runVelocity(speeds); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 78b1793c..4e9e524f 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -111,6 +112,15 @@ public Command toSetpoint(Rotation2d setpointAngle) { this); } + public Command toSetpoint(Supplier setpointAngle) { + return Commands.run( + () -> { + feedback.setGoal(setpointAngle.get().getRadians()); + feedbackMode = true; + }, + this); + } + /** * Runs the command with a specified percentage of voltage. * diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index e0cb1edd..9b92d719 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -26,14 +26,14 @@ public class ArmConstants { KG = new TunableNumber("Arm/KG", 0); KS = new TunableNumber("Arm/KS", 0.0); KV = new TunableNumber("Arm/KV", 0.0); - KP = new TunableNumber("Arm/KP", 30); + KP = new TunableNumber("Arm/KP", 35); // 66 KD = new TunableNumber("Arm/KD", 0); MAX_VELOCITY = new TunableNumber("Arm/MaxVelocity", Double.POSITIVE_INFINITY); MAX_ACCELERATION = new TunableNumber("Arm/MaxAcceleration", Units.degreesToRadians(900)); STOW = Rotation2d.fromDegrees(-13.95); GEAR_RATIO = new GearRatio(228.571429, 1); AMP_POSITION = Rotation2d.fromDegrees(78.26); - TOLERENCE = Units.degreesToRadians(0.5); + TOLERENCE = Units.degreesToRadians(0.25); AFTER_INTAKE_POS = Rotation2d.fromDegrees(-9.75); } default -> { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 8293bc51..c6a333c8 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { @@ -19,16 +20,23 @@ public Climber(ClimberIO climberIO) { super(); this.climberIO = climberIO; + climberIO.resetPosition(); } public void periodic() { climberIO.updateInputs(climberIOInputs); Logger.processInputs("Climber", climberIOInputs); + RobotState.getInstance().climberRatchet = climberIOInputs.ratchetLocked; + if (getLimitSwitchLeft() && getLimitSwitchRight()) { + climberIO.resetPosition(); + } } /** * Command to raise or lower the climber arms If percentOutput is negative, the climber will lower - * If percentOutput is positive, the climber will raise + * If percentOutput is positive, the climber will raise If percentOutput is 0, the climber will + * stop If the ratchet is locked, the climber will not move. It does nothing before the command + * and then checks if the ratchet is locked. Ends the command if the ratchet is locked. * * @param percentOutput takes a number from -1 to 1. * @return no return @@ -36,10 +44,25 @@ public void periodic() { public Command raiseOrLower(double percentOutput) { return Commands.run( () -> { + Logger.recordOutput("Climber/OutputDesired", percentOutput); climberIO.setMotorsOutputPercent(percentOutput); }, this) - .onlyWhile(() -> !climberIOInputs.ratchetLocked); + .onlyWhile(() -> !climberIOInputs.ratchetLocked) + .beforeStarting( + Commands.none() + .alongWith( + Commands.runOnce( + () -> { + RobotState.getInstance().climberUp = percentOutput > 0; + RobotState.getInstance().climberDown = percentOutput < 0; + }))) + .onlyWhile(() -> !climberIOInputs.ratchetLocked) + .finallyDo( + () -> { + RobotState.getInstance().climberUp = false; + RobotState.getInstance().climberDown = false; + }); } /** * Command to disable the climber @@ -47,7 +70,7 @@ public Command raiseOrLower(double percentOutput) { * @return no return */ public Command setRatchet(boolean locked) { - return Commands.run( + return Commands.runOnce( () -> { climberIO.setRatchetLocked(locked); }, @@ -57,4 +80,12 @@ public Command setRatchet(boolean locked) { public boolean getRatchet() { return climberIOInputs.ratchetLocked; } + + public boolean getLimitSwitchLeft() { + return climberIO.getLimitSwitchLeft(); + } + + public boolean getLimitSwitchRight() { + return climberIO.getLimitSwitchRight(); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index aaf7ed67..285595a1 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.math.util.Units; import frc.robot.Constants; public class ClimberConstants { @@ -8,15 +9,19 @@ public class ClimberConstants { public static final double CLIMBER_FORWARD_PERCENT; public static final double CLIMBER_BACKWARD_PERCENT; public static final double BACKUP_TIME; + public static final float LOWER_LIMIT_POSITION; + public static final float UPPER_LIMIT_POSITION; static { switch (Constants.getRobot()) { case ROBOT_2024_COMP -> { CLIMBER_RATCHET_ID = 0; - ROTS_TO_METERS = 1.0; + ROTS_TO_METERS = 0.00584; CLIMBER_FORWARD_PERCENT = 0.6; CLIMBER_BACKWARD_PERCENT = -0.6; - BACKUP_TIME = 0.1; + BACKUP_TIME = 0.2; + LOWER_LIMIT_POSITION = (float) Units.inchesToMeters(1); + UPPER_LIMIT_POSITION = (float) Units.inchesToMeters(16); } default -> { CLIMBER_RATCHET_ID = 0; @@ -24,6 +29,8 @@ public class ClimberConstants { CLIMBER_FORWARD_PERCENT = 0.0; CLIMBER_BACKWARD_PERCENT = 0.0; BACKUP_TIME = 0.0; + LOWER_LIMIT_POSITION = 0.0f; + UPPER_LIMIT_POSITION = 0.0f; } } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 18b65188..aa2654a7 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -37,4 +37,6 @@ default boolean getLimitSwitchRight() { default boolean getLimitSwitchLeft() { return true; } + + default void resetPosition() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java index d4c38243..b9b362c5 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java @@ -12,7 +12,6 @@ public class ClimberIOSparkMax implements ClimberIO { private final CANSparkMax climberRight; private final RelativeEncoder climberRightEncoder; private final Relay climberRatchet; - private boolean ratchetLocked = false; private SparkLimitSwitch reverseLimitSwitchLeft; private SparkLimitSwitch fowardLimitSwitchLeft; private SparkLimitSwitch reverseLimitSwitchRight; @@ -33,8 +32,20 @@ public ClimberIOSparkMax() { climberLeft.setIdleMode(CANSparkBase.IdleMode.kBrake); climberRight.enableVoltageCompensation(12); climberLeft.enableVoltageCompensation(12); - climberRight.setSmartCurrentLimit(80); - climberLeft.setSmartCurrentLimit(80); + climberRight.setSmartCurrentLimit(40); + climberLeft.setSmartCurrentLimit(40); + climberRight.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, false); + climberLeft.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, false); + climberRight.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + climberLeft.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + climberLeft.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, ClimberConstants.LOWER_LIMIT_POSITION); + climberRight.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, ClimberConstants.LOWER_LIMIT_POSITION); + climberLeft.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, ClimberConstants.UPPER_LIMIT_POSITION); + climberRight.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, ClimberConstants.UPPER_LIMIT_POSITION); // Limit Switches reverseLimitSwitchLeft = climberLeft.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); fowardLimitSwitchLeft = climberLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); @@ -43,7 +54,7 @@ public ClimberIOSparkMax() { fowardLimitSwitchRight = climberRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // Ratchet - climberRatchet = new Relay(ClimberConstants.CLIMBER_RATCHET_ID, Relay.Direction.kReverse); + climberRatchet = new Relay(ClimberConstants.CLIMBER_RATCHET_ID, Relay.Direction.kForward); climberRatchet.set(Relay.Value.kOff); } @@ -56,7 +67,7 @@ public void updateInputs(ClimberIOInputs inputs) { inputs.appliedVoltsRight = climberRight.getBusVoltage() * climberRight.getAppliedOutput(); inputs.currentAmpsRight = climberRight.getOutputCurrent(); inputs.ClimberRightPosition = climberRightEncoder.getPosition(); - inputs.ratchetLocked = ratchetLocked; + inputs.ratchetLocked = climberRatchet.get().equals(Relay.Value.kOff); inputs.reverseLimitSwitchLeftPressed = reverseLimitSwitchLeft.isPressed(); inputs.forwardLimitSwitchLeftPressed = fowardLimitSwitchLeft.isPressed(); inputs.reverseLimitSwitchRightPressed = reverseLimitSwitchRight.isPressed(); @@ -80,8 +91,7 @@ public void setRightMotorPercentOutput(double percentOutput) { } public void setRatchetLocked(boolean locked) { - climberRatchet.set(ratchetLocked ? Relay.Value.kOff : Relay.Value.kOn); - ratchetLocked = locked; + climberRatchet.set(locked ? Relay.Value.kOff : Relay.Value.kOn); } @Override @@ -93,4 +103,10 @@ public boolean getLimitSwitchLeft() { public boolean getLimitSwitchRight() { return fowardLimitSwitchRight.isPressed() || reverseLimitSwitchRight.isPressed(); } + + @Override + public void resetPosition() { + climberLeftEncoder.setPosition(0); + climberRightEncoder.setPosition(0); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index de7858bc..4408a3fe 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -112,22 +111,23 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - // Update odometry - // Update odometry SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { wheelDeltas[i] = modules[i].getPositionDelta(); } - // The twist represents the motion of the robot since the last - // loop cycle in x, y, and theta based only on the modules, - // without the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); + + // Update gyro angle if (gyroInputs.connected) { - twist = new Twist2d(twist.dx, twist.dy, gyroInputs.yaw.minus(lastGyroRotation).getRadians()); + // Use the real gyro angle lastGyroRotation = gyroInputs.yaw; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(wheelDeltas); + lastGyroRotation = lastGyroRotation.plus(new Rotation2d(twist.dtheta)); } - odometry.addDriveData(Timer.getFPGATimestamp(), twist); + + odometry.getPoseEstimator().update(lastGyroRotation, getModulePositions()); updateRobotState(); } @@ -192,15 +192,23 @@ private SwerveModuleState[] getModuleStates() { return states; } + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getPosition(); + } + return positions; + } + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { - return odometry.getLatestPose(); + return odometry.getPoseEstimator().getEstimatedPosition(); } /** Returns the current odometry rotation. */ public Rotation2d getRotation() { - return odometry.getLatestPose().getRotation(); + return odometry.getPoseEstimator().getEstimatedPosition().getRotation(); } public double getPitchVelocity() { @@ -217,11 +225,11 @@ public Rotation3d getRotation3d() { /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { - odometry.resetPose(pose); + odometry.getPoseEstimator().resetPosition(lastGyroRotation, getModulePositions(), pose); } /** Returns an array of module translations. */ - public Translation2d[] getModuleTranslations() { + public static Translation2d[] getModuleTranslations() { return new Translation2d[] { new Translation2d(DriveConstants.TRACK_WIDTH_X / 2.0, DriveConstants.TRACK_WIDTH_Y / 2.0), new Translation2d(DriveConstants.TRACK_WIDTH_X / 2.0, -DriveConstants.TRACK_WIDTH_Y / 2.0), diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f0c5f6ed..e1cc3278 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -35,10 +35,10 @@ public class DriveConstants { }; DRIVE_GEAR_RATIO = new GearRatio(6.75, 1); TURN_GEAR_RATIO = new GearRatio(12.8, 1); - DRIVE_KS = new TunableNumber("Drive/Module/DriveKS", 0.49385); - DRIVE_KV = new TunableNumber("Drive/Module/DriveKV", 2.60818); - TURN_KP = new TunableNumber("Drive/Module/TurnKP", 3.256); - TURN_KD = new TunableNumber("Drive/Module/TurnKD", 0.05); + DRIVE_KS = new TunableNumber("Drive/Module/DriveKS", 0.17181); + DRIVE_KV = new TunableNumber("Drive/Module/DriveKV", 2.2858); + TURN_KP = new TunableNumber("Drive/Module/TurnKP", 3.89); // 3.256 + TURN_KD = new TunableNumber("Drive/Module/TurnKD", 0); } case ROBOT_2024_COMP -> { TRACK_WIDTH_X = Units.inchesToMeters(9 * 2); @@ -47,7 +47,7 @@ public class DriveConstants { WHEEL_DIAMETER = Units.inchesToMeters(4); ABSOLUTE_ANGLE_OFFSET = new Rotation2d[] { - Rotation2d.fromDegrees(136.75), + Rotation2d.fromDegrees(-80.51 + 180), Rotation2d.fromDegrees(-47.11), Rotation2d.fromDegrees(5.80), Rotation2d.fromDegrees(177.01), diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a856ebb8..7b0701fb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -122,7 +122,7 @@ public SwerveModulePosition getPositionDelta() { /** Returns the module state (turn angle and drive velocity). */ public SwerveModuleState getState() { - return new SwerveModuleState(inputs.drivePositionMeters, getAngle()); + return new SwerveModuleState(inputs.driveVelocityMetersPerSec, getAngle()); } /** Returns the drive velocity in meters/sec. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMAX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMAX.java index 669ea6ed..45acd2e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMAX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMAX.java @@ -83,8 +83,8 @@ public ModuleIOSparkMAX(int index) { driveMotor.enableVoltageCompensation(12); turnMotor.enableVoltageCompensation(12); - driveMotor.setSmartCurrentLimit(40); - turnMotor.setSmartCurrentLimit(30); + driveMotor.setSmartCurrentLimit(50); + turnMotor.setSmartCurrentLimit(40); this.index = index; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 1382cd6c..c791835d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -34,7 +34,7 @@ public class IndexerConstants { INDEXER_BACKWARD_VOLTAGE = 0.0; INDEXER_LIMIT_SWITCH_ID = 13; WHEEL_DIAMETER = 4; - INDEX_SPEED = new TunableNumber("Indexer/IndexSpeed", 0.5); + INDEX_SPEED = new TunableNumber("Indexer/IndexSpeed", 0.8); INDEXER_GEAR_RATIO = new GearRatio(1.5, 1); BACKUP_SPEED = -0.3; BACKUP_TIME = 0.2; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOPhysical.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOPhysical.java index 00b1267c..a46c10d2 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOPhysical.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOPhysical.java @@ -25,6 +25,8 @@ public IndexerIOPhysical() { * IndexerConstants.INDEXER_GEAR_RATIO.getRotationsPerInput()); indexerEncoder.setPositionConversionFactor( Units.rotationsToRadians(1) * IndexerConstants.INDEXER_GEAR_RATIO.getRotationsPerInput()); + + indexer.setSmartCurrentLimit(40); } public void updateInputs(IndexerIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 2cde8497..dc6a377f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -13,7 +13,7 @@ public class IntakeConstants { static { switch (Constants.getRobot()) { case ROBOT_2024_COMP -> { - INTAKE_SPEED = new TunableNumber("Intake/IntakeSpeed", 1); // 0.75 + INTAKE_SPEED = new TunableNumber("Intake/IntakeSpeed", 1); RELEASE_SPEED = -1; STOP_SPEED = 0.0; GEAR_RATIO = new GearRatio(18, 28); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOPhysical.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOPhysical.java index 9f6b96c9..1caf67ac 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOPhysical.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOPhysical.java @@ -23,6 +23,8 @@ public IntakeIOPhysical() { intakeEncoder.setVelocityConversionFactor( Units.rotationsPerMinuteToRadiansPerSecond(1) * IntakeConstants.GEAR_RATIO.getRotationsPerInput()); + + intakeMotor.setSmartCurrentLimit(80); } // Sets speed to motor, speed range is [-1,1]. diff --git a/src/main/java/frc/robot/subsystems/led/Leds.java b/src/main/java/frc/robot/subsystems/led/Leds.java index 5da0e29d..7398b994 100644 --- a/src/main/java/frc/robot/subsystems/led/Leds.java +++ b/src/main/java/frc/robot/subsystems/led/Leds.java @@ -23,11 +23,11 @@ public enum LedMode { ESTOPPED, AUTO_FINISHED, AUTONOMOUS, - HANGING, IN_RANGE, CAN_SHOOT, SHOOTING, CONTAINING, + SHOOTER_SPEED_READY, INTAKING, LEFT_NOTE_DETECTION, RIGHT_NOTE_DETECTION, @@ -36,7 +36,12 @@ public enum LedMode { RED_ALLIANCE, LOW_BATTERY_ALERT, DISABLED, - OFF, + DEFAULT, + DUCK, + CLIMBER_UP, + CLIMBER_DOWN, + CLIMBER_SOLENOID_DISENGAGED, + OFF } @AutoLogOutput(key = "LEDs/Mode") @@ -130,15 +135,14 @@ private void updateState() { } else { mode = LedMode.DISABLED; } - } else if (false) { // TODO: need state variable for auto finished + } else if (false) { // TODO: Test this mode = LedMode.AUTO_FINISHED; - } else if (DriverStation.isAutonomous()) { // TODO: need state variable for autonomous + } else if (DriverStation.isAutonomous()) { mode = LedMode.AUTONOMOUS; - } else if (state.hanging) { - mode = LedMode.HANGING; - + } else if (state.shooterSpeedIsReady) { + mode = LedMode.SHOOTER_SPEED_READY; } else if (state.shooting && state.hasNote) { mode = LedMode.SHOOTING; @@ -154,6 +158,12 @@ private void updateState() { } else if (state.intaking) { mode = LedMode.INTAKING; + } else if (state.climberUp) { + mode = LedMode.CLIMBER_UP; + } else if (state.climberDown) { + mode = LedMode.CLIMBER_DOWN; + } else if (!state.climberRatchet) { + mode = LedMode.CLIMBER_SOLENOID_DISENGAGED; } else if (state.seeNote) { if (state.noteAngle <= -noteAngle) { // TODO: need to change to constant once angle known mode = LedMode.LEFT_NOTE_DETECTION; @@ -165,7 +175,7 @@ private void updateState() { mode = LedMode.STRAIGHT_NOTE_DETECTION; } } else { - mode = LedMode.OFF; + mode = LedMode.DEFAULT; } } @@ -173,7 +183,6 @@ private void updateLeds() { switch (mode) { case ESTOPPED: solid(Section.FULL, Color.kRed); - // strobe(Section.FULL, Color.kRed, strobeFastDuration); break; case AUTO_FINISHED: @@ -185,10 +194,6 @@ private void updateLeds() { wave(Section.FULL, Color.kGold, Color.kDarkBlue, waveFastCycleLength, waveFastDuration); break; - case HANGING: - solid(Section.FULL, new Color("#006400")); // Dark Green is 0x006400 - break; - case IN_RANGE: wave( Section.FULL, @@ -217,6 +222,9 @@ private void updateLeds() { solid(Section.FULL, Color.kGreen); break; + case SHOOTER_SPEED_READY: + strobe(Section.FULL, Color.kGreen, .1); + break; case INTAKING: // leds glow in the direction it's intaking wave( @@ -264,16 +272,39 @@ private void updateLeds() { } break; - case OFF: + case DUCK: + solid(Section.BOTTOM_HALF, Color.kYellow); + break; + + case CLIMBER_UP: + rainbow(Section.TOP_HALF, rainbowCycleLength, rainbowDuration, true); + break; + + case CLIMBER_DOWN: + rainbow(Section.BOTTOM_HALF, rainbowCycleLength, rainbowDuration, false); + break; + + case CLIMBER_SOLENOID_DISENGAGED: + rainbow(Section.FULL, rainbowCycleLength, rainbowDuration, true); + break; + + case DEFAULT: wave(Section.FULL, Color.kGold, Color.kDarkBlue, waveSlowCycleLength, waveSlowDuration); break; + case OFF: + solid(Section.FULL, Color.kBlack); + break; + default: wave(Section.FULL, Color.kGold, Color.kDarkBlue, waveSlowCycleLength, waveSlowDuration); break; } leds.setData(buffer); + // if (state.duck) { + // mode = LedMode.DUCK; + // } } @Override @@ -432,14 +463,26 @@ private void breath(Section section, Color c1, Color c2, double duration, double * @param cycleLength the length of a complete rainbow cycle in degrees * @param duration the duration of the rainbow effect in seconds */ - private void rainbow(Section section, double cycleLength, double duration) { - double x = (1 - ((Timer.getFPGATimestamp() / duration) % 1.0)) * 180.0; - double xDiffPerLed = 180.0 / cycleLength; - for (int i = 0; i < section.end(); i++) { - x += xDiffPerLed; - x %= 180.0; - if (i >= section.start()) { - buffer.setHSV(i, (int) x, 255, 255); + private void rainbow(Section section, double cycleLength, double duration, boolean up) { + if (up) { + double x = (1 - ((Timer.getFPGATimestamp() / duration) % 1.0)) * 180.0; + double xDiffPerLed = 180.0 / cycleLength; + for (int i = section.start(); i < section.end(); i++) { + x += xDiffPerLed; + x %= 180.0; + if (i >= section.start()) { + buffer.setHSV(i, (int) x, 255, 255); + } + } + } else { + double x = ((Timer.getFPGATimestamp() / duration) % 1.0) * 180.0; + double xDiffPerLed = 180.0 / cycleLength; + for (int i = section.start(); i < section.end(); i++) { + x += xDiffPerLed; + x %= 180.0; + if (i >= section.start()) { + buffer.setHSV(i, (int) x, 255, 255); + } } } } diff --git a/src/main/java/frc/robot/subsystems/pixy2/Pixy2IOPhysical.java b/src/main/java/frc/robot/subsystems/pixy2/Pixy2IOPhysical.java index 614e1588..18e6917a 100644 --- a/src/main/java/frc/robot/subsystems/pixy2/Pixy2IOPhysical.java +++ b/src/main/java/frc/robot/subsystems/pixy2/Pixy2IOPhysical.java @@ -34,7 +34,7 @@ public void updateInputs(Pixy2IOInputs inputs) { if (hasChanged(inputs)) { age = 0; } else { - age = age + 1; + age++; } inputs.age = pixyTable.getEntry("Age").getDouble(0.0); inputs.x = pixyTable.getEntry("X").getDouble(0.0); @@ -43,6 +43,6 @@ public void updateInputs(Pixy2IOInputs inputs) { inputs.signature = pixyTable.getEntry("Signature").getDouble(0.0); inputs.width = pixyTable.getEntry("Width").getDouble(0.0); inputs.height = pixyTable.getEntry("Height").getDouble(0.0); - inputs.seesNote = pixyTable.getEntry("Valid").getInteger(0) == 1 && age < 10; + inputs.seesNote = pixyTable.getEntry("Valid").getBoolean(false) && age < 1000; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1ff0d407..9587ee87 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -55,6 +55,7 @@ public void periodic() { } Logger.recordOutput("Shooter/setPointVelocity", shooterFeedback.getSetpoint()); Logger.recordOutput("Shooter/error", shooterFeedback.getVelocityError()); + robotState.shooterSpeedIsReady = ShooterSpeedIsReady(); } /** * @param velocitySetpoint the velocity that the shooter should be set to @@ -98,11 +99,10 @@ public Command manualShootVolts(double volts) { * that of the setpoint of the PID. */ public boolean ShooterSpeedIsReady() { - if (shooterFeedback.getSetpoint() == 0) { - return false; - } else { - return shooterFeedback.atSetpoint(); - } + return inputs.shooterLeftVelocityRadPerSec + >= ShooterConstants.SHOOTER_READY_VELOCITY_RAD_PER_SEC + && inputs.shooterRightVelocityRadPerSec + >= ShooterConstants.SHOOTER_READY_VELOCITY_RAD_PER_SEC; } public boolean atVelocity(double velocitySetpoint) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cd52f2d3..192f36b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -26,7 +26,7 @@ public class ShooterConstants { SHOOT_SPEED = 0.0; } case ROBOT_2024_COMP -> { - SHOOTER_READY_VELOCITY_RAD_PER_SEC = 0.0; + SHOOTER_READY_VELOCITY_RAD_PER_SEC = 450; SHOOTER_KS = new TunableNumber("Shooter/ShooterKS", 0.05); SHOOTER_KV = new TunableNumber("Shooter/ShooterKV", 0.05); SHOOTER_KP = new TunableNumber("Shooter/ShooterKP", 1.0); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPhysical.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPhysical.java index 37c5a2b0..f950629d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPhysical.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPhysical.java @@ -28,6 +28,9 @@ public ShooterIOPhysical() { shooterRightEncoder.setVelocityConversionFactor(rotsToRads / 60); shooterLeftEncoder.setPositionConversionFactor(rotsToRads); shooterRightEncoder.setPositionConversionFactor(rotsToRads); + + shooterLeft.setSmartCurrentLimit(35); + shooterRight.setSmartCurrentLimit(35); } public void updateInputs(ShooterIOInputs inputs) {