From 8feb0f3a265e80c934020afeb2a2ca92feac581e Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 17 Jan 2026 14:07:33 -0500 Subject: [PATCH 1/3] initial code copy over - its broken --- .../java/frc/robot/apriltags/ApriltagIO.java | 5 + .../frc/robot/apriltags/ApriltagInputs.java | 40 ++++++ .../frc/robot/apriltags/ApriltagReading.java | 10 ++ .../frc/robot/apriltags/MockApriltag.java | 8 ++ .../java/frc/robot/apriltags/TCPApriltag.java | 56 ++++++++ .../robot/apriltags/TCPApriltagServer.java | 42 ++++++ .../java/frc/robot/apriltags/TCPServer.java | 120 ++++++++++++++++++ 7 files changed, 281 insertions(+) create mode 100644 src/main/java/frc/robot/apriltags/ApriltagIO.java create mode 100644 src/main/java/frc/robot/apriltags/ApriltagInputs.java create mode 100644 src/main/java/frc/robot/apriltags/ApriltagReading.java create mode 100644 src/main/java/frc/robot/apriltags/MockApriltag.java create mode 100644 src/main/java/frc/robot/apriltags/TCPApriltag.java create mode 100644 src/main/java/frc/robot/apriltags/TCPApriltagServer.java create mode 100644 src/main/java/frc/robot/apriltags/TCPServer.java diff --git a/src/main/java/frc/robot/apriltags/ApriltagIO.java b/src/main/java/frc/robot/apriltags/ApriltagIO.java new file mode 100644 index 0000000..c99d1f9 --- /dev/null +++ b/src/main/java/frc/robot/apriltags/ApriltagIO.java @@ -0,0 +1,5 @@ +package frc.robot.apriltags; + +import frc.robot.utils.logging.LoggableIO; + +public interface ApriltagIO extends LoggableIO {} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/ApriltagInputs.java b/src/main/java/frc/robot/apriltags/ApriltagInputs.java new file mode 100644 index 0000000..d75e616 --- /dev/null +++ b/src/main/java/frc/robot/apriltags/ApriltagInputs.java @@ -0,0 +1,40 @@ +package frc.robot.apriltags; + +import frc.robot.utils.logging.subsystem.FolderLoggableInputs; +import org.littletonrobotics.junction.LogTable; + +public class ApriltagInputs extends FolderLoggableInputs { + public double[] timestamp = new double[0]; + public double[] serverTime = new double[0]; + public double[] posX = new double[0]; + public double[] posY = new double[0]; + public double[] poseYaw = new double[0]; + public double[] distanceToTag = new double[0]; + public int[] apriltagNumber = new int[0]; + + public ApriltagInputs(String folder) { + super(folder); + } + + @Override + public void toLog(LogTable table) { + table.put("timestamp", timestamp); + table.put("serverTime", serverTime); + table.put("posX", posX); + table.put("posY", posY); + table.put("poseYaw", poseYaw); + table.put("distanceToTag", distanceToTag); + table.put("apriltagNumber", apriltagNumber); + } + + @Override + public void fromLog(LogTable table) { + timestamp = table.get("timestamp", timestamp); + serverTime = table.get("serverTime", serverTime); + posX = table.get("posX", posX); + posY = table.get("posY", posY); + poseYaw = table.get("poseYaw", poseYaw); + distanceToTag = table.get("distanceToTag", distanceToTag); + apriltagNumber = table.get("apriltagNumber", apriltagNumber); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/ApriltagReading.java b/src/main/java/frc/robot/apriltags/ApriltagReading.java new file mode 100644 index 0000000..54def10 --- /dev/null +++ b/src/main/java/frc/robot/apriltags/ApriltagReading.java @@ -0,0 +1,10 @@ +package frc.robot.apriltags; + +public record ApriltagReading( + double posX, + double posY, + double poseYaw, + double distanceToTag, + int apriltagNumber, + double latency, + double measurementTime) {} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/MockApriltag.java b/src/main/java/frc/robot/apriltags/MockApriltag.java new file mode 100644 index 0000000..f5e22c8 --- /dev/null +++ b/src/main/java/frc/robot/apriltags/MockApriltag.java @@ -0,0 +1,8 @@ +package frc.robot.apriltags; + +import frc.robot.utils.logging.LoggableIO; + +public class MockApriltag implements LoggableIO { + @Override + public void updateInputs(ApriltagInputs inputs) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltag.java b/src/main/java/frc/robot/apriltags/TCPApriltag.java new file mode 100644 index 0000000..76eb482 --- /dev/null +++ b/src/main/java/frc/robot/apriltags/TCPApriltag.java @@ -0,0 +1,56 @@ +package frc.robot.apriltags; + +import edu.wpi.first.math.geometry.*; +import frc.robot.constants.Constants; +import frc.robot.utils.Apriltag; +import frc.robot.utils.logging.LoggableIO; +import java.util.Queue; +import org.littletonrobotics.junction.Logger; + +public class TCPApriltag implements LoggableIO { + private final TCPApriltagServer server; + + public TCPApriltag() { + server = new TCPApriltagServer(Constants.TCP_SERVER_PORT); + server.start(); + } + + @Override + public void updateInputs(ApriltagInputs inputs) { + Queue queue = server.flush(); + int queueSize = queue.size(); + Logger.recordOutput("VisionMeasurementsThisTick", queueSize); + inputs.posX = new double[queueSize]; + inputs.posY = new double[queueSize]; + inputs.poseYaw = new double[queueSize]; + inputs.distanceToTag = new double[queueSize]; + inputs.apriltagNumber = new int[queueSize]; + inputs.serverTime = new double[queueSize]; + inputs.timestamp = new double[queueSize]; + + Translation3d[] apriltagPoseArray = new Translation3d[queueSize]; + Pose2d[] visionPoseArray = new Pose2d[queueSize]; + + for (int i = 0; i < queueSize; i++) { + ApriltagReading measurement = queue.poll(); + inputs.posX[i] = measurement.posX(); + inputs.posY[i] = measurement.posY(); + inputs.poseYaw[i] = measurement.poseYaw(); + inputs.distanceToTag[i] = measurement.distanceToTag(); + inputs.apriltagNumber[i] = measurement.apriltagNumber(); + inputs.timestamp[i] = measurement.latency(); + inputs.serverTime[i] = measurement.measurementTime(); + + Apriltag apriltag = Apriltag.of(measurement.apriltagNumber()); + apriltagPoseArray[i] = + apriltag == null ? new Translation3d(0, 0, 0) : apriltag.getTranslation(); + visionPoseArray[i] = + new Pose2d( + measurement.posX(), + measurement.posY(), + Rotation2d.fromDegrees(measurement.poseYaw())); + } + Logger.recordOutput("Apriltag/TagPoses", apriltagPoseArray); + Logger.recordOutput("Apriltag/VisionPoses", visionPoseArray); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java new file mode 100644 index 0000000..cc44e9d --- /dev/null +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -0,0 +1,42 @@ +package frc.robot.apriltags; + +import edu.wpi.first.wpilibj.Timer; +import java.io.DataInputStream; +import java.io.IOException; + +public class TCPApriltagServer extends TCPServer { + + public TCPApriltagServer(int port) { + super(port); + } + + /** + * Format of message: [(double)x, (double)y, (double)yaw, (double)distance,(double)timestamp, + * (int)apriltagNumber] + */ + @Override + protected ApriltagReading extractFromStream(DataInputStream stream) throws IOException { + double posX = -1; + double posY = -1; + double poseYaw = -1; + double distanceToTag = -1; + double timestamp = -1; + int apriltagNumber = -1; + double now = 0; + while (posX == -1 + && posY == -1 + && poseYaw == -1 + && distanceToTag == -1 + && apriltagNumber == -1 + && timestamp == -1) { + posX = stream.readDouble(); + posY = stream.readDouble(); + poseYaw = stream.readDouble(); + distanceToTag = stream.readDouble(); + timestamp = stream.readDouble(); + apriltagNumber = stream.readInt(); + now = Timer.getFPGATimestamp() * 1000; + } + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPServer.java b/src/main/java/frc/robot/apriltags/TCPServer.java new file mode 100644 index 0000000..e3359da --- /dev/null +++ b/src/main/java/frc/robot/apriltags/TCPServer.java @@ -0,0 +1,120 @@ +package frc.robot.apriltags; + +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.constants.Constants; +import frc.robot.utils.logging.commands.CommandLogger; +import java.io.BufferedInputStream; +import java.io.DataInputStream; +import java.io.IOException; +import java.net.ServerSocket; +import java.net.Socket; +import java.util.LinkedList; +import java.util.Queue; +import java.util.concurrent.LinkedTransferQueue; + +public abstract class TCPServer extends Thread { + + private final int port; + private ServerSocket serverSocket; + private Socket clientSocket; + private DataInputStream dataInputStream; + private BufferedInputStream bufferedInputStream; + private boolean running = false; + private final LinkedTransferQueue readings = new LinkedTransferQueue<>(); + + /** + * @param port for network communication. Port must match between client and server (range + * 5800-5810 (rio limitations)) + */ + public TCPServer(int port) { + this.port = port; + } + + @Override + public void run() { + try { + this.serverSocket = new ServerSocket(port); + this.serverSocket.setSoTimeout(Constants.SERVER_SOCKET_CONNECTION_TIMEOUT); + this.clientSocket = null; + } catch (IOException e2) { + DriverStation.reportError("Could not start server on port " + port, true); + this.serverSocket = null; + } + running = true; + while (running) { + if (serverSocket == null) { + escape(); + continue; + } + if (this.clientSocket == null) { + clientSocket = waitForClient(); + if (clientSocket == null) { + continue; + } + try { + bufferedInputStream = new BufferedInputStream(clientSocket.getInputStream()); + dataInputStream = new DataInputStream(bufferedInputStream); + } catch (IOException e) { + DriverStation.reportError("Could not fetch client Input Stream", true); + dataInputStream = null; + continue; + } + } + try { + readings.add(extractFromStream(dataInputStream)); + } catch (IOException e) { + DriverStation.reportError("Problem while reading tcp stream data", true); + clientSocket = null; + bufferedInputStream = null; + dataInputStream = null; + } + } + } + + protected abstract T extractFromStream(DataInputStream stream) throws IOException; + + private Socket waitForClient() { + Socket s; + try { + s = serverSocket.accept(); + CommandLogger.get().logMessage("TCP Client Connection Status", true); + } catch (IOException e1) { + CommandLogger.get().logMessage("TCP Client Connection Status", false); + s = null; + } + try { + Thread.sleep(Constants.SERVER_SOCKET_ATTEMPT_DELAY); + } catch (InterruptedException e) { + DriverStation.reportError("TCP Server thread interrupted: " + e.getMessage(), true); + } + return s; + } + + public void escape() { + this.running = false; + try { + if (bufferedInputStream != null) { + bufferedInputStream.close(); + } + if (dataInputStream != null) { + dataInputStream.close(); + } + if (clientSocket != null) { + clientSocket.close(); + } + if (serverSocket != null) { + serverSocket.close(); + } + } catch (IOException e) { + DriverStation.reportError("Could not release thread resources!", true); + } finally { + CommandLogger.get().logMessage("TCP Client Connection Status", false); + } + } + + public Queue flush() { + Queue queue = new LinkedList<>(); + readings.drainTo(queue); + return queue; + } +} \ No newline at end of file From e2f30c40dc896950eeacbe458f4edf27cc3fd79f Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 17 Jan 2026 15:04:55 -0500 Subject: [PATCH 2/3] its so close to being doen --- .../java/frc/robot/apriltags/ApriltagIO.java | 4 +- .../frc/robot/apriltags/ApriltagInputs.java | 8 +-- .../frc/robot/apriltags/MockApriltag.java | 10 ++- .../java/frc/robot/apriltags/TCPApriltag.java | 8 +-- src/main/java/frc/robot/utils/Apriltag.java | 65 +++++++++++++++++++ 5 files changed, 81 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/utils/Apriltag.java diff --git a/src/main/java/frc/robot/apriltags/ApriltagIO.java b/src/main/java/frc/robot/apriltags/ApriltagIO.java index c99d1f9..c7d8aef 100644 --- a/src/main/java/frc/robot/apriltags/ApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/ApriltagIO.java @@ -1,5 +1,5 @@ package frc.robot.apriltags; -import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.io.BaseIo; -public interface ApriltagIO extends LoggableIO {} \ No newline at end of file +public interface ApriltagIO extends BaseIo {} \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/ApriltagInputs.java b/src/main/java/frc/robot/apriltags/ApriltagInputs.java index d75e616..8d0701d 100644 --- a/src/main/java/frc/robot/apriltags/ApriltagInputs.java +++ b/src/main/java/frc/robot/apriltags/ApriltagInputs.java @@ -1,9 +1,9 @@ package frc.robot.apriltags; -import frc.robot.utils.logging.subsystem.FolderLoggableInputs; +import org.littletonrobotics.junction.inputs.LoggableInputs; import org.littletonrobotics.junction.LogTable; -public class ApriltagInputs extends FolderLoggableInputs { +public class ApriltagInputs implements LoggableInputs { public double[] timestamp = new double[0]; public double[] serverTime = new double[0]; public double[] posX = new double[0]; @@ -12,9 +12,7 @@ public class ApriltagInputs extends FolderLoggableInputs { public double[] distanceToTag = new double[0]; public int[] apriltagNumber = new int[0]; - public ApriltagInputs(String folder) { - super(folder); - } + @Override public void toLog(LogTable table) { diff --git a/src/main/java/frc/robot/apriltags/MockApriltag.java b/src/main/java/frc/robot/apriltags/MockApriltag.java index f5e22c8..f32aff4 100644 --- a/src/main/java/frc/robot/apriltags/MockApriltag.java +++ b/src/main/java/frc/robot/apriltags/MockApriltag.java @@ -1,8 +1,12 @@ package frc.robot.apriltags; -import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.input.MotorLoggableInputs; +import frc.robot.utils.logging.io.BaseIoImpl; -public class MockApriltag implements LoggableIO { +public class MockApriltag extends BaseIOImpl implements ApriltagIO { + public MockApriltag(String name, ApriltagInputs inputs) { + super(name, inputs); + } @Override - public void updateInputs(ApriltagInputs inputs) {} + protected void updateInputs(ApriltagInputs inputs) {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltag.java b/src/main/java/frc/robot/apriltags/TCPApriltag.java index 76eb482..2ac1b06 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltag.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltag.java @@ -1,13 +1,13 @@ package frc.robot.apriltags; import edu.wpi.first.math.geometry.*; -import frc.robot.constants.Constants; +import frc.robot.Constants; import frc.robot.utils.Apriltag; -import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.io.BaseIoImpl; import java.util.Queue; import org.littletonrobotics.junction.Logger; -public class TCPApriltag implements LoggableIO { +public class TCPApriltag extends BaseIOImpl implements ApriltagIO{ private final TCPApriltagServer server; public TCPApriltag() { @@ -16,7 +16,7 @@ public TCPApriltag() { } @Override - public void updateInputs(ApriltagInputs inputs) { + protected void updateInputs(ApriltagInputs inputs) { Queue queue = server.flush(); int queueSize = queue.size(); Logger.recordOutput("VisionMeasurementsThisTick", queueSize); diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java new file mode 100644 index 0000000..2c36b79 --- /dev/null +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -0,0 +1,65 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Translation3d; + +public enum Apriltag { + ONE(16.687292, 0.628142, 1.4859), + TWO(16.687292, 7.414259999999999, 1.4859), + THREE(11.49096, 8.031733999999998, 1.30175), + FOUR(9.276079999999999, 6.132575999999999, 1.8679160000000001), + FIVE(9.276079999999999, 1.909825999999999, 1.8679160000000001), + SIX(13.474446, 3.3012379999999997, 0.308102), + SEVEN(13.890498, 4.0208200000000005, 0.308102), + EIGHT(13.474446, 4.740402, 0.308102), + NINE(12.643358, 4.740402, 0.308102), + TEN(12.227305999999999, 4.0208200000000005, 0.308102), + ELEVEN(12.643358, 3.3012379999999997, 0.308102), + TWELVE(0.8613139999999999, 0.628142, 1.4859), + THIRTEEN(0.8613139999999999, 7.414259999999999, 1.4859), + FOURTEEN(8.272272, 6.132575999999999, 1.8679160000000001), + FIFTEEN(8.272272, 1.9098259999999998, 1.8679160000000001), + SIXTEEN(6.057646, 0.010667999999999999, 1.30175), + SEVENTEEN(4.073905999999999, 3.3012379999999997, 0.308102), + EIGHTEEN(3.6576, 4.0208200000000005, 0.308102), + NINETEEN(4.073905999999999, 4.740402, 0.308102), + TWENTY(4.904739999999999, 4.740402, 0.308102), + TWENTY_ONE(5.321046, 4.0208200000000005, 0.17), + TWENTY_TWO(4.904739999999999, 3.3012379999999997, 0.308102); + + private final double x; + private final double y; + private final double z; + + Apriltag(double x, double y, double z) { + this.x = x; + this.y = y; + this.z = z; + } + + public static Apriltag of(int number) { + if (number > values().length || number <= 0) { + return null; + } + return Apriltag.values()[number - 1]; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getZ() { + return z; + } + + public Translation3d getTranslation() { + return new Translation3d(x, y, z); + } + + public int number() { + return ordinal(); + } +} \ No newline at end of file From 0cc5d5621497439b8f83c0cdbce76bb89b2afeab Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 17 Jan 2026 15:12:06 -0500 Subject: [PATCH 3/3] WIP --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/apriltags/MockApriltag.java | 3 +- .../java/frc/robot/apriltags/TCPApriltag.java | 7 +- .../frc/robot/commands/roller/SpinRoller.java | 2 +- .../frc/robot/commands/tilt/TiltDown.java | 2 +- .../java/frc/robot/commands/tilt/TiltUp.java | 2 +- .../frc/robot/{ => constants}/Constants.java | 4 +- .../frc/robot/constants/Constants2025.java | 53 ++++ .../frc/robot/constants/GameConstants.java | 241 ++++++++++++++++++ .../frc/robot/subsystems/RollerSubsystem.java | 2 +- .../frc/robot/subsystems/TiltSubsystem.java | 2 +- .../utils/logging/commands/CommandLogger.java | 3 +- .../utils/logging/io/motor/SimSparkMaxIo.java | 3 +- .../robot/utils/simulation/ArmSimulator.java | 3 +- .../utils/simulation/RobotVisualizer.java | 3 +- 16 files changed, 316 insertions(+), 16 deletions(-) rename src/main/java/frc/robot/{ => constants}/Constants.java (96%) create mode 100644 src/main/java/frc/robot/constants/Constants2025.java create mode 100644 src/main/java/frc/robot/constants/GameConstants.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c0d5cd2..ad24b21 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.Constants; import frc.robot.utils.logging.commands.CommandLogger; /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f664f73..c9de7c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; import frc.robot.commands.tilt.TiltUp; +import frc.robot.constants.Constants; import frc.robot.subsystems.RollerSubsystem; import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.simulation.RobotVisualizer; diff --git a/src/main/java/frc/robot/apriltags/MockApriltag.java b/src/main/java/frc/robot/apriltags/MockApriltag.java index f32aff4..30516de 100644 --- a/src/main/java/frc/robot/apriltags/MockApriltag.java +++ b/src/main/java/frc/robot/apriltags/MockApriltag.java @@ -1,9 +1,8 @@ package frc.robot.apriltags; -import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.BaseIoImpl; -public class MockApriltag extends BaseIOImpl implements ApriltagIO { +public class MockApriltag extends BaseIoImpl implements ApriltagIO { public MockApriltag(String name, ApriltagInputs inputs) { super(name, inputs); } diff --git a/src/main/java/frc/robot/apriltags/TCPApriltag.java b/src/main/java/frc/robot/apriltags/TCPApriltag.java index 2ac1b06..d6544a4 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltag.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltag.java @@ -1,16 +1,17 @@ package frc.robot.apriltags; import edu.wpi.first.math.geometry.*; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; import java.util.Queue; import org.littletonrobotics.junction.Logger; -public class TCPApriltag extends BaseIOImpl implements ApriltagIO{ +public class TCPApriltag extends BaseIoImpl implements ApriltagIO{ private final TCPApriltagServer server; - public TCPApriltag() { + public TCPApriltag(String name, ApriltagInputs inputs) { + super(name, inputs); server = new TCPApriltagServer(Constants.TCP_SERVER_PORT); server.start(); } diff --git a/src/main/java/frc/robot/commands/roller/SpinRoller.java b/src/main/java/frc/robot/commands/roller/SpinRoller.java index 539b234..8d5b89c 100644 --- a/src/main/java/frc/robot/commands/roller/SpinRoller.java +++ b/src/main/java/frc/robot/commands/roller/SpinRoller.java @@ -1,7 +1,7 @@ package frc.robot.commands.roller; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.RollerSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; diff --git a/src/main/java/frc/robot/commands/tilt/TiltDown.java b/src/main/java/frc/robot/commands/tilt/TiltDown.java index e097e5e..4208304 100644 --- a/src/main/java/frc/robot/commands/tilt/TiltDown.java +++ b/src/main/java/frc/robot/commands/tilt/TiltDown.java @@ -1,7 +1,7 @@ package frc.robot.commands.tilt; //added a comment import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; // The command makes thing tilt down diff --git a/src/main/java/frc/robot/commands/tilt/TiltUp.java b/src/main/java/frc/robot/commands/tilt/TiltUp.java index 5c3ac23..8f8f580 100644 --- a/src/main/java/frc/robot/commands/tilt/TiltUp.java +++ b/src/main/java/frc/robot/commands/tilt/TiltUp.java @@ -1,7 +1,7 @@ package frc.robot.commands.tilt; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; // This command makes thing tilt up diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/constants/Constants.java similarity index 96% rename from src/main/java/frc/robot/Constants.java rename to src/main/java/frc/robot/constants/Constants.java index f5a5839..7431ef7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot; +package frc.robot.constants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; @@ -15,7 +15,7 @@ *

It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */ -public final class Constants { +public final class Constants extends Constants2025{ public enum Mode { /** diff --git a/src/main/java/frc/robot/constants/Constants2025.java b/src/main/java/frc/robot/constants/Constants2025.java new file mode 100644 index 0000000..c9883db --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants2025.java @@ -0,0 +1,53 @@ +package frc.robot.constants; + +public class Constants2025 extends GameConstants { + /** Constants2025 is only for CANIDs and nothing else, everything else goes into GameConstants. */ + + // Elevator + public static final int ELEVATOR_MOTOR_ID = 35; + + public static final double MAX_ELEVATOR_ENCODER_POSITION = -46.9; + + // Coral Shooter + public static final int SHOOTER_MOTOR_LEADER_ID = 53; + public static final int SHOOTER_MOTOR_FOLLOWER_ID = 8; + public static final int SHOOTER_MOTOR_ALIGNER_ID = 10; + + // Algae Roller + public static final int ALGAE_ROLLER_CAN_ID = 2; + + // Algae Extender + public static final int ALGAE_EXTENDER_MOTOR_ID = 3; + + // Algae Bye-Bye + public static final int ALGAE_BYEBYE_SPINING_ID = 5; + public static final int ALGAE_BYEBYE_TILT_ID = 1; + + // Climber + public static final int CLIMBER_MOTOR_ID = 61; // TODO: change later + + // light strip + public static final int LIGHTSTRIP_PORT = 0; // TODO: change later + + // Drive + public static final int DRIVE_FRONT_RIGHT_D = 34; + public static final int DRIVE_BACK_RIGHT_D = 31; + public static final int DRIVE_FRONT_LEFT_D = 56; + public static final int DRIVE_BACK_LEFT_D = 51; + public static final int DRIVE_CANCODER_FRONT_RIGHT = 12; + public static final int DRIVE_CANCODER_BACK_RIGHT = 13; + public static final int DRIVE_CANCODER_FRONT_LEFT = 11; + public static final int DRIVE_CANCODER_BACK_LEFT = 14; + + // Steer + public static final int DRIVE_FRONT_RIGHT_S = 46; + public static final int DRIVE_BACK_RIGHT_S = 43; + public static final int DRIVE_FRONT_LEFT_S = 55; + public static final int DRIVE_BACK_LEFT_S = 49; + + // Measured distance between the center of the wheels + public static final double DRIVE_BASE_WIDTH = 0.61595; + + // Measured distance between the center of the wheels + public static final double DRIVE_BASE_LENGTH = 0.61595; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java new file mode 100644 index 0000000..a56d8b2 --- /dev/null +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -0,0 +1,241 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; + +public class GameConstants { + + // Controllers + public static final int LEFT_JOYSTICK_ID = 0; + public static final int RIGHT_JOYSTICK_ID = 1; + public static final int XBOX_CONTROLLER_ID = 2; + + // Diags + public static final double BYEBYE_ROLLER_DIAGS_ENCODER = 1; + public static final double HIHI_ROLLER_DIAGS_ENCODER = 0.3; + public static final double CORAL_ALIGNER_DIAGS_ENCODER = 0.3; + public static final double ELEVATOR_DIAGS_ENCODER = 1; + public static final double HIHI_EXTENDER_DIAGS_ENCODER = 1; + public static final double CORAL_DIAGS_ENCODER = 1; + public static final double GYRO_DIAGS_ANGLE = 30; + + // Debug + public static final boolean SWERVE_DEBUG = false; + public static final boolean INTAKE_DEBUG = false; + public static final boolean CLIMBER_DEBUG = false; + public static final boolean ELEVATOR_DEBUG = false; + public static final boolean CORAL_DEBUG = false; + public static final boolean HIHI_DEBUG = false; + public static final boolean BYEBYE_DEBUG = false; + public static final boolean COMMAND_DEBUG = false; + public static final boolean INPUTS_DEBUG = false; + public static final boolean ARM_DEBUG = false; + public static final boolean TUNING_MODE = false; + + // Speeds + public static final double MAX_AUTO_ALIGN_SPEED = 0.9; + public static final double ELEVATOR_RISE_SPEED = 0.66; + public static final double CLIMBER_PHASE1_SPEED = 0.3; // TODO: change later + public static final double CLIMBER_PHASE2_SPEED = 0.3; // TODO: change later + public static final double ELEVATOR_LOWER_SPEED = -0.5; + public static final int ALGAE_EXTENDER_MOTOR_SPEED = 4; // TODO: change later + public static final double BYEBYE_ROLLER_SPEED = 0.5; + public static final double TILT_SPEED = 0.15; + public static final double BYEBYE_FORWARD_SPEED = 0.5; // TODO: change later + public static final double BYEBYE_REVERSE_SPEED = -0.7; // TODO: change later + public static final double INTAKE_MOTOR_SPEED = 0.25; + public static final double INTAKE_ALIGNER_SPEED = 0.5; + public static final double INTAKE_TILT_VELOCITY = 0.5; + public static final double CORAL_SHOOTER_SPEED = 0.5; + public static final double HIHI_EXTEND_SPEED = 0.4; + public static final double HIHI_RETRACT_SPEED = -0.15; + public static final double HIHI_INTAKE_SPEED = 0.7; + public static final double HIHI_SHOOT_SPEED = -0.9; + public static final double CLIMBER_SPEED = 0.5; + public static final double CLIMBER_RISE_SPEED = 0.5; + public static final double HIHI_INTAKE_BASE_VELOCITY = 7000.0; + + // Accelerations + public static final double MAX_PATHPLANNER_ACCEL = 11.7; + public static final double MAX_PATHPLANNER_ANGULAR_ACCEL = 3797; + + // Timeouts + public static final int AUTO_ALIGN_TIMEOUT = 10; + public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; + public static final int ELEVATOR_TIMEOUT = 10; + public static final int ELEVATOR_RESET_TIMEOUT = 10; + public static final int BYEBYE_SPIN_ROLLER_TIMEOUT = 10; + public static final int ELEVATOR_TO_POSITION_TIMEOUT = 10; // TODO: change later + public static final int BYEBYE_FORWARD_TIMEOUT = 10; // TODO: change later + public static final int BYEBYE_REVERSE_TIMEOUT = 10; // TODO: change later + public static final double SHOOT_CORAL_TIMEOUT = 0.75; + public static final int CORAL_FWR_TIMEOUT = 10; + public static final int ROLL_ALGAE_TIMEOUT = 10; + public static final double HIHI_RETRACT_TIMEOUT = 10; + public static final double HIHI_ROLLER_OUT_TIMEOUT = 5; + public static final double HIHI_ROLLER_IN_TIMEOUT = 5; + public static final int INTAKE_CORAL_TIMEOUT = 10; + public static final int CLIMBER_PHASE2_TIMEOUT = 10; // TODO: change later + public static final int RESET_CLIMBER_TIMEOUT = 10; + public static final double HIHI_INTAKE_TIMEOUT = 10; // TODO: Change Later + public static final double INTAKE_LED_STRIP_TIME = 1; + + // Logging + public static final long MAX_LOG_TIME_WAIT = 10; + public static final boolean ENABLE_LOGGING = true; + + // Treshholds + public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; + public static final double AUTO_ALIGN_THRESHOLD = 2.3; // degrees //TODO: change later + public static final double ELEVATOR_ENCODER_THRESHHOLD = 1; // TODO: Change Later + public static final int HIHI_EXTENDER_TICK_LIMIT = 10; + + // Mode + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + /** Replaying from a log file. */ + REPLAY + } + + // Limits + public static final int DRIVE_SMART_LIMIT = 38; // TODO: change later + public static final int DRIVE_SECONDARY_LIMIT = 48; // TODO: change later + public static final double DRIVE_RAMP_RATE_LIMIT = 0.1; // TODO: change later + public static final int NEO_CURRENT_LIMIT = 20; + public static final int HIHI_CURRENT_LIMIT = 10; + + // HiHi PID + public static final double HIHI_PID_P = 0.02; // 0.08 with max motion + public static final double HIHI_PID_I = 0; + public static final double HIHI_PID_D = 0; + public static final double HIHI_PID_I_ZONE = 0; + public static final double HIHI_PID_FF = 0; + public static final double HIHI_PID_MAX_VEL = 3000; + public static final double HIHI_PID_MAX_ACC = 2500; + public static final double HIHI_PID_ALLOWED_ERROR = 0.5; + public static final boolean HIHI_USE_MAX_MOTION = false; + + // Elevator PID + public static final double ELEVATOR_PID_P = 0.03; + public static final double ELEVATOR_PID_I = 0; + public static final double ELEVATOR_PID_D = 0; + public static final double ELEVATOR_PID_FF = 0.001; + public static final double ELEVATOR_PID_IZONE = 0; + public static final double ELEVATOR_PID_MAX_VELOCITY = 3000; + public static final double ELEVATOR_PID_MAX_ACCELERATION = 30000; + public static final boolean ELEVATOR_USE_MAX_MOTION = true; + + // Drive PID + public static final double DRIVE_PID_P = 2; // TODO: change later + public static final double DRIVE_PID_I = 0; // TODO: change later + public static final double DRIVE_PID_D = 0; // TODO: change later + public static final double DRIVE_PID_FF_S = 0.19; + public static final double DRIVE_PID_FF_V = 3.3; + public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later + public static final double DRIVE_PID_ALLOWED_ERROR = 0; + + // Steer PID + public static final double STEER_PID_P = 0.3; // TODO: change later + public static final double STEER_PID_I = 0; // TODO: change later + public static final double STEER_PID_D = 0.005; // TODO: change later + public static final double STEER_PID_FF_S = 0; // 0.2; //TODO: change later + public static final double STEER_PID_FF_V = 0; // 0.8; //TODO: change later + + // pathplanner SLOW ROBOT PID CHANGE FOR COMPETITION + public static final double PATH_PLANNER_TRANSLATION_PID_P = 5; + public static final double PATH_PLANNER_TRANSLATION_PID_I = 0; + public static final double PATH_PLANNER_TRANSLATION_PID_D = 0; + public static final double PATH_PLANNER_ROTATION_PID_P = 1; + public static final double PATH_PLANNER_ROTATION_PID_I = 0; + public static final double PATH_PLANNER_ROTATION_PID_D = 0; + + // Lengths + public static final double ELEVATOR_DRUM_RADIUS = + Units.inchesToMeters(1); // In M(in), change later + public static final double MIN_ELEVATOR_HEIGHT_METERS = 0; // in m + public static final double MAX_ELEVATOR_HEIGHT_METERS = 1; // in m + public static final double INITIAL_ELEVATOR_HEIGHT = 0; // TODO: change later + public static final double HIHI_LENGTH = 0.2; // TODO: change later + public static final double BYEBYE_TILT_LENGTH = 0.2; // TODO: change later + public static final double INITIAL_CLIMBER_HEIGHT = 0.2; + public static final double CLIMBER_BASE_LENGTH = 0.2; + public static final double CLIMBER_ARM_LENGTH = 0.2; + + // Angles + public static final Rotation2d HIHI_MIN_ANGLE = Rotation2d.fromDegrees(0); + public static final Rotation2d HIHI_MAX_ANGLE = Rotation2d.fromDegrees(90); + public static final Rotation2d BYEBYE_TILT_MIN_ANGLE = + Rotation2d.fromDegrees(45); // TODO: change later + public static final Rotation2d BYEBYE_TILT_MAX_ANGLE = + Rotation2d.fromDegrees(90); // TODO: change later + public static final double BYEBYE_TILT_INERTIA = 0.5; // TODO: change later + public static final double BYEBYE_TILT_GEARING = 45.0; // TODO: change later + public static final boolean BYEBYE_TILT_SIMULATE_GRAVITY = true; + public static final double HIHI_EXTEND_POSITION = 15.5; // TODO: change later + public static final double HIHI_RETRACT_POSITION = 0.0; // TODO: change later + + // Zeros + public static final double BACK_RIGHT_ABS_ENCODER_ZERO = 0.306885; // TODO: change later + public static final double FRONT_LEFT_ABS_ENCODER_ZERO = -0.059082; // TODO: change later + public static final double BACK_LEFT_ABS_ENCODER_ZERO = 0.379150; // TODO: change later + public static final double FRONT_RIGHT_ABS_ENCODER_ZERO = -0.100586; // TODO: change later + + // Climber + public static final double CLIMBER_PHASE1_POSITION = 150.0; + public static final double CLIMBER_DEADBAND = 0.4; + public static final double CLIMBER_DEPLOY_HARPOON_TIMEOUT = 10; + + // Drivetrain + public static final double WHEEL_RADIUS = 0.0508; // TODO: change later + public static final double MAX_VELOCITY = 1.5; // 4 meters per second //TODO: change later + public static final double MAX_ANGULAR_SPEED = 6 * Math.PI; // TODO: change later + + // Other + public static final double GRAVITY = -9.81; + public static final long GYRO_THREAD_RATE_MS = 10; + public static final int SERVER_SOCKET_ATTEMPT_DELAY = 100; + public static final int TCP_SERVER_PORT = 5806; + public static final boolean ENABLE_VISION = true; + public static final long POSE_BUFFER_STORAGE_TIME = 2; + public static final double ELEVATOR_GEARING = 10; // TODO: change later + public static final double CARRIAGE_MASS = 25.4; // In Kg, change later +// public static final SwerveModuleProfileV2 SWERVE_MODULE_PROFILE = +// SwerveModuleProfileV2.MK4I; // TODO: Uncomment AND change later + + public static final double HIHI_GEARING = 15.0; // TODO: change later + public static final double HIHI_INERTIA = 0.01; // TODO: change later + public static final boolean HI_HI_SIMULATE_GRAVITY = false; + public static final int MAX_VALID_TICKS_INTAKE = 15; // TODO: Change Later + public static final int MAX_VALID_TICKS_ELEVATOR = 10; // TODO: Change Later + public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.005; // TODO: change later + + public static final double ROBOT_MASS = 58.967; // In Kg, change later + public static final double ROBOT_BUMPER_WIDTH = 0.914; + public static final double ROBOT_BUMPER_LENGTH = 0.914; + public static final double STEER_ROTATIONAL_INERTIA = 0.03; // TODO: change later + public static final double COEFFICIENT_OF_FRICTION = 1.542; + public static final double STEER_FRICTION_VOLTAGE = 0.2; + + // ELEVATOR CONSTANTS + public static final double ELEVATOR_MANUAL_DEADBAND = 0.2; + public static final double ELEVATOR_MANUAL_MAX_SPEED_UP = -.3; + public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; + + // Limelight + public static final Transform3d LIMELIGHT_TO_ROBOT = + new Transform3d( + 0.3429, -0.0635, 0, new Rotation3d(0.0, (-47 * Math.PI) / 180, 0.0)); // z = 0.720725 + public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later + public static final double MINIMUM_PIECE_DETECTION_DOT = 0; + public static final boolean ENABLE_FANCY_LIMELIGHT_MATH = true; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java index 3f929d0..934b46c 100644 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RollerSubsystem.java @@ -10,7 +10,7 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.motor.MockSparkMaxIo; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; diff --git a/src/main/java/frc/robot/subsystems/TiltSubsystem.java b/src/main/java/frc/robot/subsystems/TiltSubsystem.java index 11c2a96..93e7ea3 100644 --- a/src/main/java/frc/robot/subsystems/TiltSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TiltSubsystem.java @@ -10,7 +10,7 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.motor.MockSparkMaxIo; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; diff --git a/src/main/java/frc/robot/utils/logging/commands/CommandLogger.java b/src/main/java/frc/robot/utils/logging/commands/CommandLogger.java index 0a24732..ef31cfe 100644 --- a/src/main/java/frc/robot/utils/logging/commands/CommandLogger.java +++ b/src/main/java/frc/robot/utils/logging/commands/CommandLogger.java @@ -7,7 +7,8 @@ import edu.wpi.first.util.struct.Struct; import edu.wpi.first.util.struct.StructSerializable; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants; +import frc.robot.constants.Constants; + import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import us.hebi.quickbuf.ProtoMessage; diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java index 8402c27..1b63bdc 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java @@ -1,7 +1,8 @@ package frc.robot.utils.logging.io.motor; import com.revrobotics.spark.SparkMax; -import frc.robot.Constants; + +import frc.robot.constants.Constants; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.simulation.Simulator; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; diff --git a/src/main/java/frc/robot/utils/simulation/ArmSimulator.java b/src/main/java/frc/robot/utils/simulation/ArmSimulator.java index 347a192..74f1a3b 100644 --- a/src/main/java/frc/robot/utils/simulation/ArmSimulator.java +++ b/src/main/java/frc/robot/utils/simulation/ArmSimulator.java @@ -13,7 +13,8 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; +import frc.robot.constants.Constants; + import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; /** diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index dab54c8..a5dad17 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -3,7 +3,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.Constants; +import frc.robot.constants.Constants; + import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d;