diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..4a63cc1 --- /dev/null +++ b/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 + } + ] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d1c197b..efea88d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 032c08c..5657871 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -42,6 +43,7 @@ import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.BallCounter; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; @@ -53,10 +55,13 @@ 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; import org.photonvision.PhotonPoseEstimator.PoseStrategy; + /** * 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} @@ -64,12 +69,14 @@ * 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 Shooter shooter; private final Climber climber; private final Intake intake; + private final BallCounter ballCounter; private final Vision vision; // Controller @@ -80,6 +87,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 @@ -248,7 +259,7 @@ public RobotContainer() { break; default: - // Replayed robot, disable IO implementations + //Replayed robot, disable IO implementations drive = new Drive( new GyroIO() {}, @@ -315,7 +326,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( @@ -325,7 +336,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 diff --git a/src/main/java/frc/robot/subsystems/BallCounter.java b/src/main/java/frc/robot/subsystems/BallCounter.java new file mode 100644 index 0000000..513518b --- /dev/null +++ b/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 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; + } + } +}