2 / 5 / 26 Added & Mapped the Intake LaserCAN!

build.gradle

@@ -1,7 +1,7 @@
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.1.1"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
     id 'com.diffplug.spotless' version '6.20.0'
     id "io.freefair.lombok" version "8.6"
 }

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

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

@@ -32,11 +32,16 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
+  import au.grapplerobotics.CanBridge;
+
+
+
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
   private RobotContainer robotContainer;
 
   public Robot() {
+    CanBridge.runTCP();
     // Record metadata
     // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
     // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
@@ -130,12 +135,12 @@ public void disabledPeriodic() {}
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
+    // autonomousCommand = robotContainer.getAutonomousCommand();
 
-    // schedule the autonomous command (example)
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
-    }
+    // // schedule the autonomous command (example)
+    // if (autonomousCommand != null) {
+    //   autonomousCommand.schedule();
+    // }
   }
 
   /** This function is called periodically during autonomous. */

src/main/java/frc/robot/RobotContainer.java

@@ -13,6 +13,7 @@
 
 package frc.robot;
 
+import au.grapplerobotics.LaserCan;
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -33,6 +34,7 @@
 import frc.robot.Constants.Ports;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.BallCounter;
 import frc.robot.subsystems.Hopper;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.drive.Drive;
@@ -41,20 +43,25 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+
 import java.util.Optional;
+
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
+
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
  * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+
   // Subsystems
-  private final Drive drive;
+ private final Drive drive;
   private final Hopper hopper;
   private final Intake intake;
+  private final BallCounter ballCounter;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -64,6 +71,10 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    // Check if the robot is real before using the ball counter!
+    if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1));
+    else ballCounter = null;
+
     switch (Constants.currentMode) {
       case REAL:
         // Real robot, instantiate hardware IO implementations
@@ -151,7 +162,7 @@ public RobotContainer() {
         break;
 
       default:
-        // Replayed robot, disable IO implementations
+        //Replayed robot, disable IO implementations
         drive =
             new Drive(
                 new GyroIO() {},
@@ -207,7 +218,7 @@ private void configureButtonBindings() {
             () -> controller.getLeftX(),
             () -> -controller.getRightX()));
 
-    // Lock to 0° when A button is held
+    //Lock to 0° when A button is held
     controller
         .a()
         .whileTrue(
@@ -217,7 +228,7 @@ private void configureButtonBindings() {
                 () -> -controller.getLeftX(),
                 () -> new Rotation2d()));
 
-    // Switch to X pattern when X button is pressed
+    //Switch to X pattern when X button is pressed
     controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     // Reset gyro to 0° when B button is pressed

src/main/java/frc/robot/subsystems/BallCounter.java

@@ -0,0 +1,100 @@
+package frc.robot.subsystems;
+
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.List;
+
+public class BallCounter extends SubsystemBase {
+  private LaserCan _laserCAN;
+  private Timer timer;
+  private int ballsFired;
+  private Timer secondsSinceLastFire;
+  private List<Double> ballsData;
+  private boolean isBlocked;
+  private boolean canFire;
+  private double fireRate;
+
+  public BallCounter(LaserCan laserCan) {
+    _laserCAN = laserCan;
+
+    timer = new Timer();
+    timer.start();
+
+    ballsFired = 0;
+    secondsSinceLastFire = new Timer();
+
+    isBlocked = false;
+    canFire = false;
+
+    fireRate = 0.0;
+
+    ballsData = new ArrayList<>();
+
+    // Config for the LaserCAN
+    // try {
+    //     _laserCAN.setRangingMode(LaserCan.RangingMode.SHORT);
+    //     // _laserCAN.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+    //     // _laserCAN.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+    //     // } catch (ConfigurationFailedException e) {
+    //     // System.out.println("Configuration failed! " + e);
+    // }
+  }
+
+  public void shootBall() {
+    ballsFired++;
+    secondsSinceLastFire.reset();
+    secondsSinceLastFire.start();
+
+    ballsData.add(timer.get());
+
+    System.out.println("Ball number " + (ballsFired) + " fired!");
+  }
+
+  public boolean ballShot() {
+    Measurement measurement = _laserCAN.getMeasurement();
+    return measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm <= 30;
+  }
+
+  public double calculateFireRate() {
+    double observationTime = 1.0;
+
+    if (ballsData.size() <= 0) return fireRate;
+    if (timer.get() - ballsData.get(0) > observationTime) ballsData.remove(0);
+
+    fireRate = ballsData.size() / observationTime;
+    return ballsData.size() / observationTime;
+  }
+
+  public int getBallsShot() {
+    return ballsFired;
+  }
+
+  public double secondsSinceLastFired() {
+    return secondsSinceLastFire.get();
+  }
+
+  @Override
+  public void periodic() {
+    isBlocked = ballShot();
+    fireRate = calculateFireRate();
+
+    secondsSinceLastFire.get();
+
+    SmartDashboard.putNumber("Balls Fired", getBallsShot());
+    SmartDashboard.putNumber("Ball Fire Rate (per second)", Math.round(fireRate * 100.0) / 100.0);
+    SmartDashboard.putNumber("Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0);
+
+    if (!isBlocked && !canFire) {
+      canFire = true;
+    }
+    else if (isBlocked && canFire) {
+      shootBall();
+      canFire = false;
+    }
+  }
+}