Practice -> dev-competition

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"

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,

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": {

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": {

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": {

src/main/deploy/swerve/swervedrive.json

@@ -4,7 +4,7 @@
     "id": 13,
     "canbus": "can"
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

src/main/java/frc/robot/Robot.java

@@ -4,22 +4,30 @@
 
 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;
 
   @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();
   }
 

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,49 +14,39 @@
 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<Velocity<Distance>> 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<Command> 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<Command> autoChooser = new SendableChooser<>();
+
   public RobotContainer() {
     // Angle conversion factor = 360 / (GEAR RATIO * ENCODER RESOLUTION)
     double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8, 4096);
     // with real values
     // 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<Velocity<Distance>> 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() {