diff --git a/build.gradle b/build.gradle index 72976dd..a05124f 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 "net.ltgt.errorprone" version "3.1.0" id "com.diffplug.spotless" version "6.23.3" id "io.freefair.lombok" version "8.4" diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 87daea7..fdf5421 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -3,15 +3,15 @@ "front": -12, "left": 12 }, - "absoluteEncoderOffset": 112.060546875, + "absoluteEncoderOffset": 317.63, "drive": { "type": "sparkmax", - "id": 5, + "id": 15, "canbus": null }, "angle": { "type": "sparkmax", - "id": 6, + "id": 2, "canbus": null }, "conversionFactor": { @@ -20,8 +20,8 @@ }, "encoder": { "type": "cancoder", - "id": 3, - "canbus": "can" + "id": 2, + "canbus": "can" }, "inverted": { "drive": false, diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 8f1d6e5..f8cf31a 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -3,15 +3,15 @@ "front": -12, "left": -12 }, - "absoluteEncoderOffset": 179.6484375, + "absoluteEncoderOffset": 13.27, "drive": { "type": "sparkmax", - "id": 3, + "id": 7, "canbus": null }, "angle": { "type": "sparkmax", - "id": 4, + "id": 8, "canbus": null }, "conversionFactor": { @@ -20,7 +20,7 @@ }, "encoder": { "type": "cancoder", - "id": 1, + "id": 0, "canbus": "can" }, "inverted": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 4c67b90..b307b39 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -3,15 +3,15 @@ "front": 12, "left": 12 }, - "absoluteEncoderOffset": 193.1, + "absoluteEncoderOffset": 359.56, "drive": { "type": "sparkmax", - "id": 7, + "id": 3, "canbus": null }, "angle": { "type": "sparkmax", - "id": 8, + "id": 4, "canbus": null }, "conversionFactor": { @@ -20,7 +20,7 @@ }, "encoder": { "type": "cancoder", - "id": 0, + "id": 1, "canbus": "can" }, "inverted": { diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 660765c..12c0ddb 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -3,15 +3,15 @@ "front": 12, "left": -12 }, - "absoluteEncoderOffset": 146.25, + "absoluteEncoderOffset": 291.00, "drive": { "type": "sparkmax", - "id": 1, + "id": 5, "canbus": null }, "angle": { "type": "sparkmax", - "id": 2, + "id": 6, "canbus": null }, "conversionFactor": { @@ -20,7 +20,7 @@ }, "encoder": { "type": "cancoder", - "id": 2, + "id": 3, "canbus": "can" }, "inverted": { diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index 8594e62..47dada7 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -4,7 +4,7 @@ "id": 13, "canbus": "can" }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c..3fb2638 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,15 @@ package frc.robot; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import monologue.Logged; +import monologue.Monologue; -public class Robot extends TimedRobot { +public class Robot extends TimedRobot implements Logged { private Command m_autonomousCommand; private RobotContainer m_robotContainer; @@ -16,10 +20,14 @@ public class Robot extends TimedRobot { @Override public void robotInit() { m_robotContainer = new RobotContainer(); + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + Monologue.setupMonologue(this, "root", false, false); } @Override public void robotPeriodic() { + Monologue.updateAll(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c81accf..30f08ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,8 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; +import static monologue.Annotations.*; + +import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -12,41 +14,29 @@ import frc.robot.inputs.NoteDetector; import frc.robot.inputs.PoseEstimator; import frc.robot.lib.controllers.Thrustmaster; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.Swerve; -import io.github.jdiemke.triangulation.NotEnoughPointsException; +import frc.robot.subsystems.*; import java.io.File; import java.io.IOException; +import monologue.Logged; import org.photonvision.PhotonCamera; import swervelib.SwerveDrive; import swervelib.math.SwerveMath; import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; -public class RobotContainer { - private final Measure> maximumSpeed = Units.FeetPerSecond.of(20); - - private final Swerve drivebase; - private final PoseEstimator poseEstimator; - private final Intake intake; - private final Shooter shooter; - private final Indexer indexer; - private final NoteDetector noteDetector; - private final ShooterDataTable shooterDataTable; // TODO: Ensure to get the actual points - +public class RobotContainer implements Logged { private static final Thrustmaster leftThrustmaster = new Thrustmaster(0); private static final Thrustmaster rightThrustmaster = new Thrustmaster(1); - private final SwerveDrive swerveDrive; - private final Superstructure superstructure; - private final SendableChooser autoChooser = new SendableChooser<>(); - private final PhotonCamera photonCamera = new PhotonCamera("photonvision"); // TODO: Remember to replace with the actual camera name static { SwerveDriveTelemetry.verbosity = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; } + private final Vision vision = new Vision(); + private final Swerve drivebase; + private final Superstructure superstructure; + private final SendableChooser autoChooser = new SendableChooser<>(); + public RobotContainer() { // Angle conversion factor = 360 / (GEAR RATIO * ENCODER RESOLUTION) double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8, 4096); @@ -54,7 +44,9 @@ public RobotContainer() { // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * // ENCODER RESOLUTION). double driveConversionFactor = SwerveMath.calculateMetersPerRotation(0, 6.75, 1); + SwerveDrive swerveDrive; try { + Measure> maximumSpeed = Units.FeetPerSecond.of(20); swerveDrive = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")) .createSwerveDrive( @@ -65,12 +57,35 @@ public RobotContainer() { throw new RuntimeException(e); } - try { - shooterDataTable = new ShooterDataTable(null, null); - } catch (NotEnoughPointsException e) { - throw new RuntimeException(e); - } + // TODO: Ensure to get the actual points + ShooterDataTable shooterDataTable; + Translation2d[] dummyPoints = + new Translation2d[] {new Translation2d(), new Translation2d(), new Translation2d()}; + ShooterSpec[] dummySpecs = + new ShooterSpec[] { + new ShooterSpec( + Units.Degrees.of(0), + Units.DegreesPerSecond.of(0), + Units.DegreesPerSecond.of(0), + Units.Degrees.of(0)), + new ShooterSpec( + Units.Degrees.of(0), + Units.DegreesPerSecond.of(0), + Units.DegreesPerSecond.of(0), + Units.Degrees.of(0)), + new ShooterSpec( + Units.Degrees.of(0), + Units.DegreesPerSecond.of(0), + Units.DegreesPerSecond.of(0), + Units.Degrees.of(0)) + }; + shooterDataTable = new ShooterDataTable(dummyPoints, dummySpecs, false); + PoseEstimator poseEstimator; + Intake intake; + Shooter shooter; + Hood hood; + NoteDetector noteDetector; try { poseEstimator = new PoseEstimator( @@ -79,26 +94,26 @@ public RobotContainer() { swerveDrive.swerveDrivePoseEstimator::getEstimatedPosition, swerveDrive::getModulePositions, swerveDrive.swerveDrivePoseEstimator::update); + // TODO: Remember to replace with the actual camera name + PhotonCamera photonCamera = new PhotonCamera("photonvision"); noteDetector = new NoteDetector(photonCamera, poseEstimator); intake = new Intake(); - shooter = new Shooter(shooterDataTable, poseEstimator); - indexer = new Indexer(shooterDataTable, poseEstimator); + TimeOfFlight sensor = new TimeOfFlight(15); + sensor.setRangingMode(TimeOfFlight.RangingMode.Short, 0.02); + + PowerBudget power = new PowerBudget(); + shooter = new Shooter(shooterDataTable, poseEstimator, sensor, power); + hood = new Hood(shooterDataTable, poseEstimator, sensor, power); } catch (IOException e) { throw new RuntimeException(e); } - try { - drivebase = - new Swerve( - swerveDrive, - new ShooterDataTable(new Translation2d[] {}, new ShooterSpec[] {}), - poseEstimator); - } catch (NotEnoughPointsException e) { - throw new RuntimeException(e); // GG go next - } - - superstructure = new Superstructure(intake, indexer, shooter, drivebase, noteDetector, poseEstimator); + drivebase = new Swerve(swerveDrive, shooterDataTable, poseEstimator); + + superstructure = + new Superstructure( + intake, hood, shooter, drivebase, noteDetector, poseEstimator, shooterDataTable); Command driveFieldOrientedDirectAngle = drivebase.driveCommand( @@ -108,14 +123,16 @@ public RobotContainer() { drivebase.setDefaultCommand(driveFieldOrientedDirectAngle); configureBindings(); - autoChooser.addOption("Top with amp", superstructure.fromTopWithAmp()); - autoChooser.addOption("Top with no amp", superstructure.fromTopWithoutAmp()); - autoChooser.addOption("Bottom with no amp", superstructure.fromBottomWithoutAmp()); - autoChooser.addOption("Middle with no amp", superstructure.fromStartingMiddleWithoutAmp()); + // autoChooser.addOption("Top with amp", superstructure.fromTopWithAmp()); + // autoChooser.addOption("Top with no amp", superstructure.fromTopWithoutAmp()); + // autoChooser.addOption("Bottom with no amp", superstructure.fromBottomWithoutAmp()); + // autoChooser.addOption("Middle with no amp", + // superstructure.fromStartingMiddleWithoutAmp()); } private void configureBindings() { rightThrustmaster.getButton(Thrustmaster.Button.TRIGGER).onTrue(drivebase.resetHeading()); + leftThrustmaster.getButton(Thrustmaster.Button.TRIGGER).onTrue(superstructure.shoot()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/ShooterDataTable.java b/src/main/java/frc/robot/ShooterDataTable.java index 550c8e8..58b0e06 100644 --- a/src/main/java/frc/robot/ShooterDataTable.java +++ b/src/main/java/frc/robot/ShooterDataTable.java @@ -4,17 +4,21 @@ import static edu.wpi.first.units.Units.RPM; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import frc.robot.lib.BarycentricInterpolation; -import io.github.jdiemke.triangulation.NotEnoughPointsException; +import java.util.Optional; +import java.util.stream.Collectors; +import java.util.stream.Stream; public class ShooterDataTable { - private BarycentricInterpolation interpolatorAngle; - private BarycentricInterpolation interpolatorSpeedL; - private BarycentricInterpolation interpolatorSpeedR; - private BarycentricInterpolation interpolatorOffset; + private final BarycentricInterpolation interpolatorAngle; + private final BarycentricInterpolation interpolatorSpeedL; + private final BarycentricInterpolation interpolatorSpeedR; + private final BarycentricInterpolation interpolatorOffset; - public ShooterDataTable(Translation2d[] points, ShooterSpec[] specs) - throws NotEnoughPointsException { + public ShooterDataTable(Translation2d[] points, ShooterSpec[] specs, Boolean verboseLogging) { double[] x = new double[points.length]; double[] y = new double[points.length]; double[] angle = new double[points.length]; @@ -30,17 +34,17 @@ public ShooterDataTable(Translation2d[] points, ShooterSpec[] specs) offset[i] = specs[i].offset().in(Degrees); } try { - interpolatorAngle = new BarycentricInterpolation(x, y, angle); - interpolatorSpeedL = new BarycentricInterpolation(x, y, speedL); - interpolatorSpeedR = new BarycentricInterpolation(x, y, speedR); - interpolatorOffset = new BarycentricInterpolation(x, y, offset); + interpolatorAngle = new BarycentricInterpolation(x, y, angle, verboseLogging); + interpolatorSpeedL = new BarycentricInterpolation(x, y, speedL, verboseLogging); + interpolatorSpeedR = new BarycentricInterpolation(x, y, speedR, verboseLogging); + interpolatorOffset = new BarycentricInterpolation(x, y, offset, verboseLogging); } catch (Exception e) { throw new RuntimeException(e); } } // testing - public static void main(String... args) throws NotEnoughPointsException { + public static void main(String... args) { Translation2d[] points = new Translation2d[] { new Translation2d(0, 0), @@ -55,15 +59,27 @@ public static void main(String... args) throws NotEnoughPointsException { new ShooterSpec(Degrees.of(2), RPM.of(2), RPM.of(1), Degrees.of(2)), new ShooterSpec(Degrees.of(2), RPM.of(2), RPM.of(2), Degrees.of(2)) }; - ShooterDataTable table = new ShooterDataTable(points, specs); - System.out.println(table.get(new Translation2d(0.5, 0.5))); + ShooterDataTable table = new ShooterDataTable(points, specs, false); + System.out.println(table.get(new Translation2d(0.5, 0.5)).get()); } - public ShooterSpec get(Translation2d toTarget) { - return new ShooterSpec( - Degrees.of(interpolatorAngle.interpolate(toTarget.getX(), toTarget.getY())), - RPM.of(interpolatorSpeedL.interpolate(toTarget.getX(), toTarget.getY())), - RPM.of(interpolatorSpeedR.interpolate(toTarget.getX(), toTarget.getY())), - Degrees.of(interpolatorOffset.interpolate(toTarget.getX(), toTarget.getY()))); + public Optional get(Translation2d toTarget) { + return Stream.of( + interpolatorAngle.interpolate(toTarget.getX(), toTarget.getY()).map(Degrees::of), + interpolatorSpeedL.interpolate(toTarget.getX(), toTarget.getY()).map(RPM::of), + interpolatorSpeedR.interpolate(toTarget.getX(), toTarget.getY()).map(RPM::of), + interpolatorOffset.interpolate(toTarget.getX(), toTarget.getY()).map(Degrees::of)) + .collect( + Collectors.collectingAndThen( + Collectors.toList(), + list -> + list.stream().allMatch(Optional::isPresent) + ? Optional.of( + new ShooterSpec( + (Measure) list.get(0).get(), + (Measure>) list.get(1).get(), + (Measure>) list.get(2).get(), + (Measure) list.get(3).get())) + : Optional.empty())); } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2d511a6..081a69c 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,134 +1,185 @@ package frc.robot; +import static edu.wpi.first.wpilibj2.command.Commands.sequence; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.inputs.NoteDetector; import frc.robot.inputs.PoseEstimator; -import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Hood; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; - -public class Superstructure extends SubsystemBase { - private final Intake intake; - private final Indexer indexer; - private final Shooter shooter; - private final Swerve swerve; - private final NoteDetector noteDetector; +import lombok.Getter; +import monologue.Logged; + +public class Superstructure extends SubsystemBase implements Logged { + @Getter private final Intake intake; + @Getter private final Hood hood; + @Getter private final Shooter shooter; + @Getter private final Swerve swerve; private final PoseEstimator poseEstimator; - - private final PathPlannerPath leftTopToNoteToAmpTraj = PathPlannerPath.fromChoreoTrajectory("leftTopToAmp"); - private final PathPlannerPath topLeftToMiddle1 = PathPlannerPath.fromChoreoTrajectory("topLeftToMiddle1"); - - private final PathPlannerPath middle1ToMiddle2 = PathPlannerPath.fromChoreoTrajectory("middle1ToMiddle2"); - private final PathPlannerPath middle1ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle1ToSpeaker"); - private final PathPlannerPath middle2ToMiddle3 = PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle3"); - private final PathPlannerPath middle2ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle2ToSpeaker"); - private final PathPlannerPath middle3ToMiddle4 = PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle4"); - private final PathPlannerPath middle3ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle3ToSpeaker"); - private final PathPlannerPath middle4ToMiddle5 = PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle5"); - private final PathPlannerPath middle4ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle4ToSpeaker"); - private final PathPlannerPath middle5ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle6"); + private final ShooterDataTable dataTable; + + private final PathPlannerPath leftTopToNoteToAmpTraj = + PathPlannerPath.fromChoreoTrajectory("leftTopToAmp"); + private final PathPlannerPath topLeftToMiddle1 = + PathPlannerPath.fromChoreoTrajectory("topLeftToMiddle1"); + + private final PathPlannerPath middle1ToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("middle1ToMiddle2"); + private final PathPlannerPath middle1ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle1ToSpeaker"); + private final PathPlannerPath middle2ToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle3"); + private final PathPlannerPath middle2ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle2ToSpeaker"); + private final PathPlannerPath middle3ToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle4"); + private final PathPlannerPath middle3ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle3ToSpeaker"); + private final PathPlannerPath middle4ToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle5"); + private final PathPlannerPath middle4ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle4ToSpeaker"); private final PathPlannerPath ampToMiddle1 = PathPlannerPath.fromChoreoTrajectory("ampToMiddle1"); - private final PathPlannerPath bottomLeftToMiddle5 = PathPlannerPath.fromChoreoTrajectory("bottomLeftToMiddle5"); - - private final PathPlannerPath middle5ToMiddle4 = PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle4"); - private final PathPlannerPath middle4ToMiddle3 = PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle3"); - private final PathPlannerPath middle3ToMiddle2 = PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle2"); - private final PathPlannerPath middle2ToMiddle1 = PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle1"); - private final PathPlannerPath speakerToMiddle2 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle2"); - private final PathPlannerPath speakerToMiddle3 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle3"); - private final PathPlannerPath speakerToMiddle4 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle4"); - private final PathPlannerPath speakerToMiddle5 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle5"); - - private final PathPlannerPath fromStartingMiddleToMiddle3 = PathPlannerPath - .fromChoreoTrajectory("fromStartingMiddleToMiddle3"); + private final PathPlannerPath bottomLeftToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("bottomLeftToMiddle5"); + + private final PathPlannerPath middle5ToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle4"); + private final PathPlannerPath middle4ToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle3"); + private final PathPlannerPath middle3ToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle2"); + private final PathPlannerPath middle2ToMiddle1 = + PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle1"); + private final PathPlannerPath speakerToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle2"); + private final PathPlannerPath speakerToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle3"); + private final PathPlannerPath speakerToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle4"); + private final PathPlannerPath speakerToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle5"); + + private final PathPlannerPath fromStartingMiddleToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("fromStartingMiddleToMiddle3"); + private State state = State.NO_NOTE; + private State.RangeStatus rangeStatus = State.RangeStatus.OUTSIDE_RANGE; public Superstructure( Intake intake, - Indexer indexer, + Hood hood, Shooter shooter, Swerve swerve, NoteDetector noteDetector, - PoseEstimator poseEstimator) { + PoseEstimator poseEstimator, + ShooterDataTable dataTable) { this.intake = intake; - this.indexer = indexer; + this.hood = hood; this.shooter = shooter; this.swerve = swerve; - this.noteDetector = noteDetector; this.poseEstimator = poseEstimator; + this.dataTable = dataTable; } + // TODO: test whether we can actually move notes from the intake to the hood while the hood is + // angled @Override public void periodic() { - ParallelCommandGroup toDo = new ParallelCommandGroup(); + switch (state) { + case NO_NOTE -> { + if (intake.hasNote()) { + state = State.HAS_NOTE; + break; + } + + intake.on(); + switch (rangeStatus) { // if we know what the hood angle should be, adjust + case IN_RANGE -> hood.adjusting(); + case OUTSIDE_RANGE -> hood.idle(); + } + } + + case HAS_NOTE -> { + intake.off(); // hold note in intake + switch (rangeStatus) { + case IN_RANGE -> hood.adjusting(); + case OUTSIDE_RANGE -> hood.idle(); + } + } + + case SHO0TING -> { + switch (rangeStatus) { + case IN_RANGE -> { + swerve.faceSpeaker().schedule(); + if (shooter.ready() && hood.ready() && swerve.ready()) + intake.on(); // run intake so note goes into hood/shooter/indexer/shootdexer + } - if (intake.isNoteFound()) { - toDo.addCommands(indexer.startLoading()); - } - if (indexer.isInactive() && intake.isNotePassed()) { // fired note - toDo.addCommands(intake.startIntake()); + case OUTSIDE_RANGE -> { + // log?, should never happen + } + } + if (intake.empty() && shooter.empty()) + state = State.NO_NOTE; // when we are done shooting reset state + } } - toDo.schedule(); + rangeStatus = + dataTable.get(poseEstimator.translationToSpeaker()).isPresent() + ? State.RangeStatus.IN_RANGE + : State.RangeStatus.OUTSIDE_RANGE; + } + + public Command shoot() { + return runOnce(() -> state = State.SHO0TING) + .onlyIf(intake::hasNote) + .onlyIf(() -> rangeStatus == State.RangeStatus.IN_RANGE); } - public Command fireShot() { - return shooter - .requestShot() - .alongWith(swerve.faceSpeaker()) - .andThen( - shooter - .waitUntilReady() - .alongWith(indexer.waitUntilReady()) - .andThen(indexer.fire().alongWith(shooter.fire()))); + public Command cancelShot() { + return runOnce( + intake.empty() && shooter.empty() + ? () -> state = State.NO_NOTE + : () -> state = State.HAS_NOTE); } public Command fromTopWithAmp() { - return new SequentialCommandGroup( - intake.startIntake(), - fromLeftTopToNoteToAmp(), - fireShot(), - intake.startIntake(), - fromAmpToMiddle1(), - fromMiddle1ToMiddle5()); + return sequence(fromLeftTopToNoteToAmp(), shoot(), fromAmpToMiddle1(), fromMiddle1ToMiddle5()); } public Command fromTopWithoutAmp() { - return intake.startIntake().andThen(fromTopToMiddle1()).andThen(fromMiddle1ToMiddle5()); + return fromTopToMiddle1().andThen(fromMiddle1ToMiddle5()); } public Command fromBottomWithoutAmp() { - return intake.startIntake().andThen(fromBottomToMiddle5()).andThen(fromMiddle5ToMiddle1()); + return fromBottomToMiddle5().andThen(fromMiddle5ToMiddle1()); } public Command fromStartingMiddleWithoutAmp() { - return new SequentialCommandGroup( - intake.startIntake(), + return sequence( fromStartingMiddleToMiddle3(), checkAndHandleNote(fromMiddle3ToSpeaker(), fromMiddle3ToMiddle2(), fromSpeakerToMiddle2()), checkAndHandleNote( fromMiddle2ToSpeaker(), fromMiddle2ToMiddle3(), null) // NOTE: we could do more... - ); + ); } public Command fromMiddle1ToMiddle5() { - return new SequentialCommandGroup( + return sequence( checkAndHandleNote(fromMiddle1ToSpeaker(), fromMiddle1ToMiddle2(), fromSpeakerToMiddle2()), checkAndHandleNote(fromMiddle2ToSpeaker(), fromMiddle2ToMiddle3(), fromSpeakerToMiddle3()), checkAndHandleNote(fromMiddle3ToSpeaker(), fromMiddle3ToMiddle4(), fromSpeakerToMiddle4()), - checkAndHandleNote(fromMiddle4ToSpeaker(), fromMiddle4ToMiddle5(), fromSpeakerToMiddle5()), - checkAndHandleNote(fromMiddle5ToSpeaker(), null, null)); + checkAndHandleNote(fromMiddle4ToSpeaker(), fromMiddle4ToMiddle5(), fromSpeakerToMiddle5())); } public Command fromMiddle5ToMiddle1() { - return new SequentialCommandGroup( - checkAndHandleNote(fromMiddle5ToMiddle4(), fromMiddle5ToSpeaker(), fromSpeakerToMiddle4()), + return sequence( checkAndHandleNote(fromMiddle4ToMiddle3(), fromMiddle4ToSpeaker(), fromSpeakerToMiddle3()), checkAndHandleNote(fromMiddle3ToMiddle2(), fromMiddle3ToSpeaker(), fromSpeakerToMiddle2()), checkAndHandleNote(fromMiddle2ToMiddle1(), fromMiddle2ToSpeaker(), fromSpeakerToMiddle2()), @@ -139,18 +190,19 @@ private Command checkAndHandleNote( Command toSpeaker, Command toNextNoteDirectly, Command fromSpeakerToNextNote) { return defer( () -> { - if (intake.isNoteFound()) { - return new SequentialCommandGroup( + // No next action if at the last note and no note is present + if (intake.hasNote()) { + return sequence( // onTheFlyRobotToNote(), - toSpeaker, fireShot(), intake.startIntake(), fromSpeakerToNextNote); - } else if (toNextNoteDirectly != null) { - return new SequentialCommandGroup(intake.startIntake(), toNextNoteDirectly); - } else { - return null; // No next action if at the last note and no note is present - } + toSpeaker, shoot(), fromSpeakerToNextNote); + } else return toNextNoteDirectly; }); } + private Command fromAmpToMiddle1() { + return AutoBuilder.followPath(ampToMiddle1); + } + // private Command onTheFlyRobotToNote() { // Translation2d robotPosition = poseEstimator.getPose().getTranslation(); // Translation2d notePosition = noteDetector.getClosestNoteTranslation().get(); @@ -175,10 +227,6 @@ private Command checkAndHandleNote( // return AutoBuilder.followPath(pathToNote); // } - private Command fromAmpToMiddle1() { - return AutoBuilder.followPath(ampToMiddle1); - } - private Command fromLeftTopToNoteToAmp() { return AutoBuilder.followPath(leftTopToNoteToAmpTraj); } @@ -219,10 +267,6 @@ private Command fromMiddle4ToSpeaker() { return AutoBuilder.followPath(middle4ToSpeaker); } - private Command fromMiddle5ToSpeaker() { - return AutoBuilder.followPath(middle5ToSpeaker); - } - private Command fromMiddle5ToMiddle4() { return AutoBuilder.followPath(middle5ToMiddle4); } @@ -262,4 +306,15 @@ private Command fromSpeakerToMiddle5() { private Command fromStartingMiddleToMiddle3() { return AutoBuilder.followPath(fromStartingMiddleToMiddle3); } + + private enum State { + NO_NOTE, + HAS_NOTE, + SHO0TING; + + private enum RangeStatus { + IN_RANGE, + OUTSIDE_RANGE + } + } } diff --git a/src/main/java/frc/robot/inputs/PoseEstimator.java b/src/main/java/frc/robot/inputs/PoseEstimator.java index 8ad43d9..5f9dc91 100644 --- a/src/main/java/frc/robot/inputs/PoseEstimator.java +++ b/src/main/java/frc/robot/inputs/PoseEstimator.java @@ -21,8 +21,6 @@ public class PoseEstimator implements Subsystem { private static final Translation2d BLUE_SPEAKER_POSITION = new Translation2d(-1.5, 218.42); private static final Translation2d RED_SPEAKER_POSITION = new Translation2d(652.73, 218.42); - private final AprilTagFieldLayout aprilTagFieldLayout; - private final Transform3d robotToCam; private final PhotonPoseEstimator photonPoseEstimator; private final Pigeon2 gyro; private final BiConsumer addVisionMeasurement; @@ -43,10 +41,10 @@ public PoseEstimator( this.getModulePositions = getModulePositions; this.update = update; gyro = new Pigeon2(13, "*"); - aprilTagFieldLayout = + AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - robotToCam = + Transform3d robotToCam = new Transform3d( new Translation3d( Units.inchesToMeters(14.5), Units.inchesToMeters(4.5), Units.inchesToMeters(18)), diff --git a/src/main/java/frc/robot/lib/BarycentricInterpolation.java b/src/main/java/frc/robot/lib/BarycentricInterpolation.java index 766bb7c..1a6b730 100644 --- a/src/main/java/frc/robot/lib/BarycentricInterpolation.java +++ b/src/main/java/frc/robot/lib/BarycentricInterpolation.java @@ -5,15 +5,19 @@ import io.github.jdiemke.triangulation.Triangle2D; import io.github.jdiemke.triangulation.Vector2D; import java.util.List; +import java.util.Optional; public class BarycentricInterpolation { private final DelaunayTriangulator triangulator; private final double[] z; + private final boolean verboseLogging; - public BarycentricInterpolation(double[] x, double[] y, double[] z) + public BarycentricInterpolation(double[] x, double[] y, double[] z, boolean verboseLogging) throws NotEnoughPointsException { this.z = z; + this.verboseLogging = verboseLogging; + List vertices = new java.util.ArrayList<>(List.of()); for (int i = 0; i < x.length; i++) { vertices.add(new Vector2D(x[i], y[i])); @@ -22,14 +26,34 @@ public BarycentricInterpolation(double[] x, double[] y, double[] z) triangulator.triangulate(); } - public double interpolate(double xVal, double yVal) { - System.out.println(triangulator.getTriangles()); + public static void main(String[] args) throws NotEnoughPointsException { + // Example usage + double[] xValues = {0, 0, 1}; + double[] yValues = {0, 1, 0}; + double[] zValues = {10, 20, 30}; + + BarycentricInterpolation interpolator = + new BarycentricInterpolation(xValues, yValues, zValues, true); + + Optional result = interpolator.interpolate(0, 0); + System.out.println("Interpolated value: " + result.get()); + } + + public Optional interpolate(double xVal, double yVal) { + if (verboseLogging) { + System.out.println(triangulator.getTriangles()); + } var triangles = triangulator.getTriangles(); - Triangle2D containingTriangle = + Optional optionalContainingTriangle = triangles.stream() .filter(triangle -> contains(triangle, new Vector2D(xVal, yVal))) - .findFirst() - .orElseThrow(); + .findFirst(); + + if (optionalContainingTriangle.isEmpty()) { + return Optional.empty(); + } + + var containingTriangle = optionalContainingTriangle.get(); Vector2D p1 = containingTriangle.a; Vector2D p2 = containingTriangle.b; @@ -42,9 +66,10 @@ public double interpolate(double xVal, double yVal) { double gamma = 1 - alpha - beta; // Interpolate z value - return alpha * z[triangles.indexOf(containingTriangle) * 3] - + beta * z[triangles.indexOf(containingTriangle) * 3 + 1] - + gamma * z[triangles.indexOf(containingTriangle) * 3 + 2]; + return Optional.of( + alpha * z[triangles.indexOf(containingTriangle) * 3] + + beta * z[triangles.indexOf(containingTriangle) * 3 + 1] + + gamma * z[triangles.indexOf(containingTriangle) * 3 + 2]); } private boolean contains(Triangle2D triangle, Vector2D vector2D) { @@ -69,16 +94,4 @@ private boolean isOn(Vector2D b, Vector2D c, Vector2D vector2D) { double dotProduct = bc.dot(bv); return !(dotProduct < 0) && !(dotProduct > bc.mag() * bc.mag()); // Outside the line segment } - - public static void main(String[] args) throws NotEnoughPointsException { - // Example usage - double[] xValues = {0, 0, 1}; - double[] yValues = {0, 1, 0}; - double[] zValues = {10, 20, 30}; - - BarycentricInterpolation interpolator = new BarycentricInterpolation(xValues, yValues, zValues); - - double result = interpolator.interpolate(0, 0); - System.out.println("Interpolated value: " + result); - } } diff --git a/src/main/java/frc/robot/lib/SimpleVelocitySystem.java b/src/main/java/frc/robot/lib/SimpleVelocitySystem.java index 78c014e..23bd9bf 100644 --- a/src/main/java/frc/robot/lib/SimpleVelocitySystem.java +++ b/src/main/java/frc/robot/lib/SimpleVelocitySystem.java @@ -8,13 +8,14 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; import edu.wpi.first.math.system.plant.LinearSystemId; +import lombok.Getter; /** Defines a simple velocity system using a kalman filter and a linear quadratic regulator */ public class SimpleVelocitySystem { private final double kS; private final double maxControlEffort; // volts - private final LinearSystem system; + @Getter private final LinearSystem system; private final LinearQuadraticRegulator regulator; private final KalmanFilter filter; private final LinearSystemLoop loop; @@ -48,6 +49,8 @@ public SimpleVelocitySystem( } /** + * Sets output + * * @param output The desired output of the system */ public void set(double output) { @@ -55,6 +58,8 @@ public void set(double output) { } /** + * Updates the system + * * @param current The measured velocity by the encoder */ public void update(double current) { @@ -64,6 +69,8 @@ public void update(double current) { } /** + * Gets output + * * @return The percent output that the controller should run at */ public double getOutput() { @@ -75,10 +82,6 @@ public double getVelocity() { return filteredVelocity; // loop.getXHat(0); } - public LinearSystem getSystem() { - return system; - } - public LinearQuadraticRegulator getLQR() { return regulator; } diff --git a/src/main/java/frc/robot/lib/TunableNumber.java b/src/main/java/frc/robot/lib/TunableNumber.java new file mode 100644 index 0000000..f31f74e --- /dev/null +++ b/src/main/java/frc/robot/lib/TunableNumber.java @@ -0,0 +1,24 @@ +package frc.robot.lib; + +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +// source: +// https://github.com/Arctos6135/frc-2024/blob/master/src/main/java/frc/robot/util/TunableNumber.java +public class TunableNumber { + private static final String TABLE_ROOT = "TunableNumbers"; + + private final DoubleEntry entry; + + public TunableNumber(String name, double defaultValue) { + entry = + NetworkTableInstance.getDefault() + .getTable(TABLE_ROOT) + .getDoubleTopic(name) + .getEntry(defaultValue); + } + + public double get() { + return entry.get(); + } +} diff --git a/src/main/java/frc/robot/lib/controllers/Thrustmaster.java b/src/main/java/frc/robot/lib/controllers/Thrustmaster.java index b14740a..9d601cf 100644 --- a/src/main/java/frc/robot/lib/controllers/Thrustmaster.java +++ b/src/main/java/frc/robot/lib/controllers/Thrustmaster.java @@ -41,4 +41,4 @@ public enum Button { val = i; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index 2605dd2..0000000 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,66 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Climber extends SubsystemBase { - private final int LEFT_MOTOR_ID = 15; // TODO: add motor ports - private final int RIGHT_MOTOR_ID = 16; - private TalonFX leftMotor; - private TalonFX rightMotor; - - public Climber() { - leftMotor = new TalonFX(LEFT_MOTOR_ID); - rightMotor = new TalonFX(RIGHT_MOTOR_ID); - leftMotor.setInverted(true); // TODO: Change based on change - rightMotor.setInverted(true); - leftMotor.setNeutralMode(NeutralModeValue.Brake); - rightMotor.setNeutralMode(NeutralModeValue.Brake); - } - - private void setTelescopeSpeed(double speed) { - leftMotor.set(speed); - rightMotor.set(speed); - } - - public Command telescopeUp() { - return new StartEndCommand( - () -> { - setTelescopeSpeed(0.5); - }, - () -> { - setTelescopeSpeed(0); - }, - this); - } - - public Command telescopeDown() { - return new StartEndCommand( - () -> { - setTelescopeSpeed(-0.5); - }, - () -> { - setTelescopeSpeed(0); - }, - this); - } - - public Command leftUp() { - return new StartEndCommand(() -> leftMotor.set(0.5), () -> leftMotor.set(0), this); - } - - public Command leftDown() { - return new StartEndCommand(() -> leftMotor.set(-0.5), () -> leftMotor.set(0), this); - } - - public Command rightUp() { - return new StartEndCommand(() -> rightMotor.set(0.5), () -> rightMotor.set(0), this); - } - - public Command rightDown() { - return new StartEndCommand(() -> rightMotor.set(-0.5), () -> rightMotor.set(0), this); - } -} diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java new file mode 100644 index 0000000..bef71eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -0,0 +1,184 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; +import static monologue.Annotations.Log; + +import com.playingwithfusion.TimeOfFlight; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.ShooterDataTable; +import frc.robot.ShooterSpec; +import frc.robot.inputs.PoseEstimator; +import frc.robot.lib.TunableNumber; +import lombok.Getter; +import monologue.Logged; + +public class Hood extends SubsystemBase implements Logged { + private static final int LEAD_ANGLE_MOTOR_ID = 14; + private static final int FOLLOW_ANGLE_MOTOR_ID = 13; + private static final double kV = 5.26; // TODO: Sysid + private static final double kA = 0.01; // TODO: sysid + private static final double kS = 0; + private static final double kG = 0.09; + private static final Measure ANGLE_STANDARD_DEVIATION = Degrees.of(10); + private static final Measure ANGLE_ERROR_TOLERANCE = Degrees.of(1.4); + private static final Measure> ANGLE_SPEED_STANDARD_DEVIATION = + Degrees.of(10).per(Seconds); + private static final Measure> ANGLE_SPEED_ERROR_TOLERANCE = + Degrees.of(1.3).per(Seconds); + private static final Measure TICKS_TO_ANGLE = Rotations.of(1.0 / 11340.0); + private static final Measure MAX_ANGLE_MOTOR_VOLTAGE = Units.Volts.of(12); + private static final Measure