From bd91fc5141e96afae2c907b822959b29a5a7bb5b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 17:50:20 -0800 Subject: [PATCH 01/18] Auto Test --- build.gradle | 3 + .../java/frc4388/robot/RobotContainer.java | 17 +- .../robot/constants/BuildConstants.java | 12 +- .../java/frc4388/robot/subsystems/Lidar.java | 309 +++++++++++++ .../frc4388/robot/subsystems/RPLidarA1.java | 431 ++++++++++++++++++ .../frc4388/utility/status/FaultA1M8.java | 39 ++ 6 files changed, 804 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Lidar.java create mode 100644 src/main/java/frc4388/robot/subsystems/RPLidarA1.java create mode 100644 src/main/java/frc4388/utility/status/FaultA1M8.java diff --git a/build.gradle b/build.gradle index bdf2b90..489a591 100644 --- a/build.gradle +++ b/build.gradle @@ -75,6 +75,9 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + implementation("com.fazecast:jSerialComm:2.4.1") + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c65108e..186b168 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; @@ -65,6 +66,7 @@ public class RobotContainer { // public final DigitalInput m_armLimitSwitch = new DigitalInput(9); /* Subsystems */ + public final Lidar m_lidar = new Lidar(); public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); //Testing of Colors public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); @@ -97,7 +99,19 @@ public class RobotContainer { private Command RobotIntakeDown = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended)) ); - + + private Command LidarIntake = new SequentialCommandGroup( + new RunCommand( + () -> m_robotSwerveDrive.driveWithInputRotation( + m_lidar.getClosestBall(), + m_lidar.getLatestBallAngle() + ), + m_robotSwerveDrive + ) + .withTimeout(10.0) + .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + ); + private Command RobotShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.setShooterReady()), new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), @@ -143,6 +157,7 @@ public class RobotContainer { }, true); NamedCommands.registerCommand("Robot Shoot", RobotShoot); + NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ebec1b4..33fa8a8 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 75; - public static final String GIT_SHA = "4907e0c8a0fc7dade91b2075d70a2b38213f9cab"; - public static final String GIT_DATE = "2026-02-21 15:08:32 MST"; - public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-21 15:52:02 MST"; - public static final long BUILD_UNIX_TIME = 1771714322479L; + public static final int GIT_REVISION = 79; + public static final String GIT_SHA = "ff0cff819cc7280a353d7ce86999efe16661f33b"; + public static final String GIT_DATE = "2026-02-23 16:58:14 MST"; + public static final String GIT_BRANCH = "AutoTesting"; + public static final String BUILD_DATE = "2026-02-23 17:39:29 MST"; + public static final long BUILD_UNIX_TIME = 1771893569353L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java new file mode 100644 index 0000000..d6685d9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -0,0 +1,309 @@ +package frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.LinkedList; +import java.util.List; +import java.util.Queue; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.highgui.HighGui; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.first.cscore.OpenCvLoader; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.RPLidarA1.PolarPoint; +import frc4388.robot.subsystems.RPLidarA1.ScanListener; +import frc4388.utility.configurable.ConfigurableDouble; +import frc4388.utility.status.FaultA1M8; + + + +public class Lidar extends SubsystemBase implements ScanListener { + // private final Spark m_motor; + private final RPLidarA1 lidar; + + private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.7); + + static + { + // This is so libopencv_javaVERSION.so (where version is the 3-digit opencv + // version) gets loaded. + try { + OpenCvLoader.forceLoad(); + } + catch (Exception e) { + e.printStackTrace(); + } + } + + // private static final double m_Scan = 0.1; + public Lidar() { + // Spark motor = new Spark(0); + // this.m_motor = motor; + this.lidar = new RPLidarA1(); + this.lidar.setListener(this); + + + // Thread processThread = new Thread(this::pointLoop); + // processThread.setDaemon(true); + // processThread.setName("RPLidar-Calc"); + // processThread.start(); + + FaultA1M8.addDevice(lidar, "A1M8"); + } + + public Rotation2d getLatestBallAngle() { + return latestBallAngleDeg; + } + + + @Override + public void periodic() { + this.lidar.setSpeed(speed.get()); + SmartDashboard.putString("lidar state", this.lidar.getStatus().toString()); + } + + // Detection constriants: cluster detection + private static final double ANG_MAX_GAP = 3.; // Degrees + private static final double DIST_MAX_GAP = 0.04; // Meters + + // Detection constraints: Circle detection + private static final double RADIUS_X_COEFF = Units.inchesToMeters(0); + private static final double RADIUS_Y_COEFF = Units.inchesToMeters(0); + private static final double RADIUS_OFFSET = Units.inchesToMeters(3); + private static final double RADIUS_TOLERANCE = Units.inchesToMeters(0.8); + + private static boolean radiusInTolerance(double x, double y, double radius) { + double rad_at_position = RADIUS_X_COEFF*x + RADIUS_Y_COEFF*y + RADIUS_OFFSET; + + return Math.abs(rad_at_position - radius) <= RADIUS_TOLERANCE; + } + + // Window constants + private static final int WIDTH = 512; + private static final int HEIGHT = 512; + private static final int POINT_RAD = 2; + + + Translation2d closestBall; + Translation2d closestBallPrior = null; + + @AutoLogOutput + public Translation2d getClosestBall() { + return closestBall; + } + + + + private List point_group = new ArrayList<>(); + private double last_ang = 0; + private double last_dist = 0; + private Rotation2d latestBallAngleDeg= new Rotation2d(); + private boolean last_color = false; + + Point LIDAR = new Point(WIDTH/2,WIDTH/2); + + @Override + public void onScanComplete(List scan) { + + System.out.println("SCAN: " + scan.size()); + + double scale = 0.006; + + List circlePoints = new ArrayList<>(); + + Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3); + + for(PolarPoint point_polar : scan) { + double ang_rad = Math.toRadians(point_polar.angle); + double x = point_polar.distance * Math.cos(ang_rad); + double y = point_polar.distance * Math.sin(ang_rad); + + // Point point_xy = new Point((WIDTH/2) + (x/scale), (HEIGHT/2) + (y/scale)); + Point point_xy = new Point(x, y); + + if( + Math.abs(last_ang - point_polar.angle) > ANG_MAX_GAP || + Math.abs(last_dist - point_polar.distance) > DIST_MAX_GAP + ) { + last_color = !last_color; + + if ( + point_group.size() >= 3 + // point_group.size() <= POINT_MAX.get() + ) { + // Get points + Point p1 = point_group.get(0); + Point p2 = point_group.get(point_group.size()/2); + Point p3 = point_group.get(point_group.size()-1); + + // Simplify to var + double dx23 = p2.x - p3.x; + double dy23 = p2.y - p3.y; + + double dx13 = p1.x - p3.x; + double dy13 = p1.y - p3.y; + + double dx12 = p1.x - p2.x; + double dy12 = p1.y - p2.y; + + // Calc Determinite + double D = p1.x*dy23 - p1.y*dx23 + (p2.x*p3.y - p3.x*p2.y); + + // The points are in a straight line. + if(D == 0) { + continue; + } + + // Square distances between each set of 2 points + double a_sq = dx23*dx23 + dy23*dy23; + double b_sq = dx13*dx13 + dy13*dy13; + double c_sq = dx12*dx12 + dy12*dy12; + + // Calculate the radius + double radius = Math.sqrt(a_sq*b_sq*c_sq) / (2 * Math.abs(D)); + + // Square distances between each point and origin + double d1 = p1.x*p1.x + p1.y*p1.y; + double d2 = p2.x*p2.x + p2.y*p2.y; + double d3 = p3.x*p3.x + p3.y*p3.y; + + // Calculate X and Y + double cx = (d1*dy23 - d2*dy13 + d3*dy12)/(2*D); + double cy = -(d1*dx23 - d2*dx13 + d3*dx12)/(2*D); + + + + if(radiusInTolerance(cx, cy, radius)) { + circlePoints.add(new Translation2d(cx, cy)); + } + + + + + // FitResult result = TaubinCircleFitter.fit(point_group, Lidar::getRadius); + + // if(result.rmsError < ERROR_BOUND.get()) { + // circlePoints.add(result.center); + // Imgproc.circle(mat, result.center, (int) (result.radius/scale), new Scalar(255,255,255)); + // } + } + + point_group.clear(); + } + + point_group.add(point_xy); + + last_ang = point_polar.angle; + last_dist = point_polar.distance; + + Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale)); + Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127)); + } + + for(Translation2d circle : circlePoints) { + Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale)); + Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255)); + // System.out.println(circle.x + " - " + circle.y); + } + + + + closestBall = new Translation2d(); + + if (circlePoints.isEmpty()) { + closestBall = new Translation2d(Double.NaN, Double.NaN); + } else { + double minDist = Double.POSITIVE_INFINITY; + Translation2d best = null; + for (Translation2d circle : circlePoints) { + double dist = circle.getSquaredNorm(); // distance from 0,0 + if (dist < minDist) { + minDist = dist; + best = circle; + } + } + + closestBall = best; + } + + if (closestBallPrior != null) { + if (closestBall.getDistance(closestBallPrior) < 0.1){ + + Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale)); + + Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); + + // System.out.println("Drive "+ Units.metersToInches(closestBall.x) + " inches forward and " + Units.metersToInches(closestBall.y) + "inches to the right"); + latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180); + System.out.println("!!" + latestBallAngleDeg); + + } else { + Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale)); + Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); + } + } + + closestBallPrior = closestBall; + + + + Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1); + + // System.o + + + showWindow(mat); + } + + private static void showWindow(Mat img) { + // Display the image in a window titled "Original Image" + HighGui.imshow("Original Image", img); + + + // Wait for a key press to close the window + HighGui.waitKey(1); + } + + // XYZ Position of the LiDAR on the robot + private static final Translation2d LiDAR_POS = new Translation2d(1, 0); + + // Angle of the lidar unit + private static final double LiDAR_PITCH = 0; // Radians + private static final double LiDAR_ROLL = 0; // Radians + + // Convert a LiDAR ball position to a field position + public static Translation2d lidarPosToField(Translation2d p, Pose2d pose) { + // Project the point tilted plane on to the XY plane + // Point should be relative to the XY plane, with (0,0) centered at the centerpoint of the lidar + double x = p.getX() * Math.cos(LiDAR_ROLL) + p.getY() * Math.sin(LiDAR_PITCH) * Math.sin(LiDAR_ROLL); + double y = p.getY() * Math.cos(LiDAR_PITCH); + + // Translate the ball position to relative to the center of the robot + // Point should be relative to robot, wth (0,0) centered at center of robot + x -= LiDAR_POS.getX(); + y -= LiDAR_POS.getY(); + + // Rotate the point by the robot's rotation + // Point should now be relative to robot, but rotated relative to the field. + double ang = -pose.getRotation().getRadians(); + x = x*Math.cos(ang) - y*Math.sin(ang); + y = x*Math.sin(ang) + y*Math.cos(ang); + + // Translate the point to the robot's field position + // Point should be relative to field. (0,0) should be relative to the field. + x += pose.getX(); + y += pose.getY(); + + return new Translation2d(x, y); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/RPLidarA1.java new file mode 100644 index 0000000..fa5aa32 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/RPLidarA1.java @@ -0,0 +1,431 @@ +package frc4388.robot.subsystems; + +import com.fazecast.jSerialComm.SerialPort; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import java.io.InputStream; +import java.io.OutputStream; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; + +/** + * Robust RPLidar A1 / R1M8 Driver for FRC. + * Implements standard protocol with auto-reconnection and state monitoring. + */ +public class RPLidarA1 { + + // --- Data Types --- + + public static class PolarPoint { + public final double angle; // Degrees 0-360 + public final double distance; // Meters + + public PolarPoint(double angle, double distance) { + this.angle = angle; + this.distance = distance; + } + } + + @FunctionalInterface + public interface ScanListener { + void onScanComplete(List scan); + } + + public enum ConnectionStatus { + DISCONNECTED, // Port not found or closed + CONNECTING, // Attempting to open serial port + CONNECTED_IDLE, // Port open, but scan not started / no data yet + CONNECTED_DISABLED,// Robot is disabled, but sensor is connected + RECEIVING_DATA, // Actively receiving valid scan points + ERROR // Communication failure or timeout + } + + // --- Protocol Constants --- + private static final byte SYNC_BYTE = (byte) 0xA5; + private static final byte SYNC_BYTE2 = (byte) 0x5A; + private static final byte CMD_STOP = (byte) 0x25; + private static final byte CMD_RESET = (byte) 0x40; + private static final byte CMD_SCAN = (byte) 0x20; + private static final byte CMD_GET_HEALTH = (byte) 0x52; + + private static final int DESCRIPTOR_LEN = 7; + private static final int SCAN_PACKET_LEN = 5; + + // --- Settings --- + private static final String PORT_DESC = "CP2102 USB to UART Bridge Controller"; + private static final double WATCHDOG_TIMEOUT = 2.5; // Seconds before assuming link is dead + + // --- Members --- + private final AtomicReference mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED); + private SerialPort mSerialPort; + private InputStream mIn; + private OutputStream mOut; + private ScanListener mListener; + + private final List mCurrentScan = new ArrayList<>(); + private double mLastDataTimestamp = 0; + // private boolean mScanningActive = false; + + public RPLidarA1() { + Thread driverThread = new Thread(this::runLoop); + driverThread.setDaemon(true); + driverThread.setName("RPLidar-Driver-Thread"); + driverThread.start(); + + Thread pwmThread = new Thread(this::funnyDTR_PWM); + pwmThread.setDaemon(true); + pwmThread.setName("RPLidar-Driver-PWM"); + pwmThread.start(); + } + + /** Sets the function to call whenever a full 360-degree rotation is parsed. */ + public void setListener(ScanListener listener) { + this.mListener = listener; + } + + // Set Speed of motor between 0 - 1 + public void setSpeed(double speed) { + this.motor_percentage = speed; + } + + public ConnectionStatus getStatus() { + return mStatus.get(); + } + + /** Signals the Lidar to stop the motor and laser. */ + private void stop_motor() { + sendCmd(CMD_RESET); + Timer.delay(0.02); + sendCmd(CMD_STOP); + mSerialPort.setDTR(); + } + + + private final static double TOGGLE_DELAY = 10; + private double motor_percentage = 0.5; + private boolean is_dtr = false; + + // Control the speed of the motor like a PWM through the DTR serial pin + // This is "PWM", like we control the speed through the percentage. + // The rate of toggles is the resolution + private void funnyDTR_PWM() { + while (!Thread.interrupted()) { + try { + ConnectionStatus status = mStatus.get(); + if (status == ConnectionStatus.RECEIVING_DATA) { + + // If the motor is at full speed + if (motor_percentage >= 1) { + // Set the motor to on + mSerialPort.clearDTR(); + // check again in a little bit + Thread.sleep(100); + } + + + // If the motor is at zero speed + if (motor_percentage <= 0) { + // Set the motor to on + mSerialPort.setDTR(); + // check again in a little bit + Thread.sleep(100); + } + + if (is_dtr) { + mSerialPort.clearDTR(); + // Sleep for main part of motor pulse + Thread.sleep((long) (TOGGLE_DELAY * motor_percentage)); + } else { + mSerialPort.setDTR(); + // Sleep for gap of motor pulse + Thread.sleep((long) (TOGGLE_DELAY * (1 - motor_percentage))); + } + + is_dtr = !is_dtr; + + } else if(status == ConnectionStatus.CONNECTED_DISABLED) { + // Stop the motor + mSerialPort.setDTR(); + + + // Sleep until we can check again + Thread.sleep(100); + + } else { // When the motor is not ready + // Sleep until we can check again + Thread.sleep(100); + } + } catch (Exception e) { + continue; + } + } + + } + + private void runLoop() { + while (!Thread.interrupted()) { + + try { + ConnectionStatus current = mStatus.get(); + + boolean robotEnabled = DriverStation.isEnabled(); + + + switch (current) { + case DISCONNECTED: + case ERROR: + attemptConnection(); + break; + + case CONNECTING: + // Handled by attemptConnection + break; + + case CONNECTED_DISABLED: + if (robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + // On enable, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + break; + } + + // We have to check the health seperately because + // the connection check only ever occurs when + // the robot is recieving data + if (!getHealth()) { + mStatus.set(ConnectionStatus.ERROR); + } + + break; + + case CONNECTED_IDLE: + if (!robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + // On enable, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + break; + } + + if (initiateScanMode()) { + mStatus.set(ConnectionStatus.RECEIVING_DATA); + mLastDataTimestamp = Timer.getFPGATimestamp(); + } else { + + mStatus.set(ConnectionStatus.ERROR); + } + + break; + + case RECEIVING_DATA: + if (!robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + break; + } + + processIncomingData(); + + + checkWatchdog(); + break; + } + + Thread.sleep(200); + + } catch (Exception e) { + continue; + } + } + } + + private void attemptConnection() { + if (mSerialPort != null && mSerialPort.isOpen()) { + mSerialPort.closePort(); + } + + mStatus.set(ConnectionStatus.CONNECTING); + + SerialPort[] ports = SerialPort.getCommPorts(); + for (SerialPort p : ports) { + if (p.getPortDescription().contains(PORT_DESC)) { + mSerialPort = p; + break; + } + } + + if (mSerialPort != null) { + mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY); + mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED); + if (mSerialPort.openPort()) { + mIn = mSerialPort.getInputStream(); + mOut = mSerialPort.getOutputStream(); + + if (DriverStation.isEnabled()) { + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + // On start, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + } else { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + stop_motor(); + } + + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + + // For A1: DTR False starts motor, DTR True stops it. + // mSerialPort.setDTR(); + return; + } + } + + mStatus.set(ConnectionStatus.DISCONNECTED); + Timer.delay(1.0); // Wait before retry + } + + private boolean initiateScanMode() { + try { + // Clear buffer before starting + while (mIn.available() > 0) mIn.read(); + + mSerialPort.clearDTR(); // Start Motor + + Thread.sleep(100); + + sendCmd(CMD_SCAN); + + // Wait for 7-byte descriptor + byte[] descriptor = new byte[DESCRIPTOR_LEN]; + long start = System.currentTimeMillis(); + while (mIn.available() < DESCRIPTOR_LEN) { + if (System.currentTimeMillis() - start > 1000) return false; + Timer.delay(0.01); + } + + mIn.read(descriptor); + + return descriptor[0] == SYNC_BYTE && descriptor[1] == SYNC_BYTE2; + } catch (Exception e) { + return false; + } + } + + private void processIncomingData() { + try { + while (mIn.available() >= SCAN_PACKET_LEN) { + byte[] packet = new byte[SCAN_PACKET_LEN]; + mIn.read(packet); + + // Protocol validation based on provided Python logic + boolean newScan = (packet[0] & 0x1) != 0; + boolean invNewScan = ((packet[0] >> 1) & 0x1) != 0; + int checkBit = (packet[1] & 0x1); + + if (newScan == invNewScan || checkBit != 1) { + // Out of sync - skip one byte to try and find sync again + return; + } + + mLastDataTimestamp = Timer.getFPGATimestamp(); + + // Python logic: ((raw[1] >> 1) + (raw[2] << 7)) / 64. + int angleRaw = ((packet[1] & 0xFF) >> 1) + ((packet[2] & 0xFF) << 7); + double angle = angleRaw / 64.0; + + // Python logic: (raw[3] + (raw[4] << 8)) / 4. (in mm) + int distRaw = (packet[3] & 0xFF) + ((packet[4] & 0xFF) << 8); + double distanceMeters = distRaw / 4000.0; + + if (newScan && !mCurrentScan.isEmpty()) { + if (mListener != null) { + mListener.onScanComplete(new ArrayList<>(mCurrentScan)); + } + mCurrentScan.clear(); + } + + if (distanceMeters > 0) { + mCurrentScan.add(new PolarPoint(angle, distanceMeters)); + } + } + } catch (Exception e) { + mStatus.set(ConnectionStatus.ERROR); + } + } + + private void checkWatchdog() { + if (Timer.getFPGATimestamp() - mLastDataTimestamp > WATCHDOG_TIMEOUT) { + + // // + // stop_motor(); + + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + + + // We have to check the health seperately because + // the connection check only ever occurs when + // the robot is recieving data + // if (!getHealth()) { + + // DriverStation.reportWarning("RPLidar A1: Data timeout. Reconnecting...", false); + // mStatus.set(ConnectionStatus.ERROR); + + // } + } + } + + private void sendCmd(byte cmd) { + try { + if (mOut != null) { + mOut.write(new byte[]{SYNC_BYTE, cmd}); + mOut.flush(); + } + } catch (Exception e) { + mStatus.set(ConnectionStatus.ERROR); + } + } + + /** + * Queries the device health status. + * @return true if the device is connected and returns a 'Good' health status, false otherwise. + */ + public boolean getHealth() { + if (mStatus.get() == ConnectionStatus.DISCONNECTED || mOut == null || mIn == null) { + return false; + } + + try { + // Ensure the buffer is clear before sending request + while (mIn.available() > 0) mIn.read(); + + sendCmd(CMD_GET_HEALTH); + + // Read 7-byte Descriptor + byte[] descriptor = new byte[DESCRIPTOR_LEN]; + long startTime = System.currentTimeMillis(); + while (mIn.available() < DESCRIPTOR_LEN) { + if (System.currentTimeMillis() - startTime > 500) return false; + Timer.delay(0.01); + } + mIn.read(descriptor); + + return true; + + // // Check if descriptor is valid and data type matches HEALTH (0x06) + // if (descriptor[0] != SYNC_BYTE || descriptor[1] != SYNC_BYTE2 || descriptor[6] != 0x06) { + // return false; + // } + + // // Read 3-byte Health Payload + // byte[] healthPayload = new byte[3]; + // while (mIn.available() < 3) { + // if (System.currentTimeMillis() - startTime > 1000) return false; + // Timer.delay(0.01); + // } + // mIn.read(healthPayload); + + // Byte 0 is status: 0x00 = Good, 0x01 = Warning, 0x02 = Error + // return healthPayload[0] == 0; + } catch (Exception e) { + return false; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java new file mode 100644 index 0000000..aedb866 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -0,0 +1,39 @@ +package frc4388.utility.status; + +import frc4388.robot.subsystems.RPLidarA1; +import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus; +import frc4388.utility.status.Status.ReportLevel; + +// Fault reporter for the RPLidar A1M8 Revolving lidar sensor +public class FaultA1M8 implements Queryable { + private String name; + private RPLidarA1 cam; + + public static void addDevice(RPLidarA1 cam, String name) { + FaultA1M8 p = new FaultA1M8(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + ConnectionStatus cam_ConnectionStatus = cam.getStatus(); + + if(cam_ConnectionStatus != ConnectionStatus.RECEIVING_DATA) + s.addReport(ReportLevel.ERROR, "Not Connected! Status = " + cam_ConnectionStatus); + + s.addReport(ReportLevel.INFO, cam.getStatus().toString()); + + return s; + } +} From 40b229296349f8942e73ea0c23abff190b53b395 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 18:09:28 -0800 Subject: [PATCH 02/18] Hopeful Tuesday Auto Test --- .../pathplanner/autos/Auto Test Slow.auto | 19 ------- .../deploy/pathplanner/autos/Monday Test.auto | 31 +++++++++++ .../autos/Shoot and Lidar test.auto | 49 +++++++++++++++++ .../{TurnNinety.path => Depot Shoot.path} | 18 +++---- .../deploy/pathplanner/paths/DriveDepot.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/Quick Shoot.path | 45 ++++++---------- .../java/frc4388/robot/RobotContainer.java | 1 + 7 files changed, 161 insertions(+), 56 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Auto Test Slow.auto create mode 100644 src/main/deploy/pathplanner/autos/Monday Test.auto create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto rename src/main/deploy/pathplanner/paths/{TurnNinety.path => Depot Shoot.path} (77%) create mode 100644 src/main/deploy/pathplanner/paths/DriveDepot.path diff --git a/src/main/deploy/pathplanner/autos/Auto Test Slow.auto b/src/main/deploy/pathplanner/autos/Auto Test Slow.auto deleted file mode 100644 index bb1c386..0000000 --- a/src/main/deploy/pathplanner/autos/Auto Test Slow.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TurnNinety" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Monday Test.auto b/src/main/deploy/pathplanner/autos/Monday Test.auto new file mode 100644 index 0000000..40d8425 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Monday Test.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Quick Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Lidar Intake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto b/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto new file mode 100644 index 0000000..21b7fe6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Quick Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "DriveDepot" + } + }, + { + "type": "named", + "data": { + "name": "Lidar Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/Depot Shoot.path similarity index 77% rename from src/main/deploy/pathplanner/paths/TurnNinety.path rename to src/main/deploy/pathplanner/paths/Depot Shoot.path index b8ac2c4..c631d45 100644 --- a/src/main/deploy/pathplanner/paths/TurnNinety.path +++ b/src/main/deploy/pathplanner/paths/Depot Shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.0, - "y": 4.0 + "x": 1.4401807228915664, + "y": 5.608590361445783 }, "prevControl": null, "nextControl": { - "x": 4.0, - "y": 4.0 + "x": 1.4401807228915664, + "y": 4.710738110733342 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.0, + "x": 2.5, "y": 4.0 }, "prevControl": { - "x": 2.0, - "y": 4.0 + "x": 2.5, + "y": 4.270156342259512 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.1, + "maxVelocity": 0.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 180.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/DriveDepot.path b/src/main/deploy/pathplanner/paths/DriveDepot.path new file mode 100644 index 0000000..f98a2c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DriveDepot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.5000000000000004, + "y": 7.854276578779859 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.47854216867347843, + "y": 6.9745542168703265 + }, + "prevControl": { + "x": 0.34740963855421675, + "y": 7.44444578313253 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index 69333b6..da1ca69 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.48951566951567, - "y": 5.107378917378918 + "x": 3.636650602409638, + "y": 5.051277108433735 }, "prevControl": null, "nextControl": { - "x": 2.908105413105413, - "y": 6.205598290598291 + "x": 2.871710843373494, + "y": 5.215192771084337 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.616082621082621, - "y": 5.107378917378918 + "x": 2.0958433734939756, + "y": 4.87643373493976 }, "prevControl": { - "x": 0.44392059766331804, - "y": 6.342550994319343 + "x": 2.2160481927710847, + "y": 5.357253012048193 }, "nextControl": { - "x": 2.234130952380953, - "y": 4.456107142857142 + "x": 1.8780822166771538, + "y": 4.00538910767248 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8064047619047625, - "y": 3.8622380952380952 + "x": 2.5, + "y": 4.0 }, "prevControl": { - "x": 2.8172023809523816, - "y": 4.132178571428572 + "x": 2.5, + "y": 4.270156342259512 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.612, - "y": 4.0213534 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.9140287769784197, - "maxWaypointRelativePos": 2, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 0.5, @@ -69,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.591140271194531 + "rotation": 180.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 178.40885972880554 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 186b168..c3e51c2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,6 +101,7 @@ public class RobotContainer { ); private Command LidarIntake = new SequentialCommandGroup( + RobotIntakeDown, new RunCommand( () -> m_robotSwerveDrive.driveWithInputRotation( m_lidar.getClosestBall(), From 7b7483138740a9a48e2d9f3ae1ae84a34fe73106 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 18:30:01 -0800 Subject: [PATCH 03/18] Tuesday Hopeful Auto Test --- .../{Monday Test.auto => Tuesday Test.auto} | 0 .../deploy/pathplanner/paths/Quick Shoot.path | 16 ++++++++-------- 2 files changed, 8 insertions(+), 8 deletions(-) rename src/main/deploy/pathplanner/autos/{Monday Test.auto => Tuesday Test.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/Monday Test.auto b/src/main/deploy/pathplanner/autos/Tuesday Test.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Monday Test.auto rename to src/main/deploy/pathplanner/autos/Tuesday Test.auto diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index da1ca69..9bbf6cd 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.0958433734939756, - "y": 4.87643373493976 + "x": 2.3958487874465044, + "y": 4.7919115549215405 }, "prevControl": { - "x": 2.2160481927710847, - "y": 5.357253012048193 + "x": 2.4481210651242846, + "y": 5.284764458740615 }, "nextControl": { - "x": 1.8780822166771538, - "y": 4.00538910767248 + "x": 2.30527817403709, + "y": 3.937960057061342 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.0 }, "prevControl": { - "x": 2.5, - "y": 4.270156342259512 + "x": 2.4025543758309, + "y": 4.230226736784185 }, "nextControl": null, "isLocked": false, From 760b6c0802477643b3812ee7936a670e14626c6c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 18:32:15 -0800 Subject: [PATCH 04/18] Delete TestRobot.java --- .../frc4388/robot/subsystems/TestRobot.java | 244 ------------------ 1 file changed, 244 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/TestRobot.java diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java deleted file mode 100644 index 4bf3dba..0000000 --- a/src/main/java/frc4388/robot/subsystems/TestRobot.java +++ /dev/null @@ -1,244 +0,0 @@ -package frc4388.robot.subsystems; - -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.configurable.ConfigurableDouble; - -public class TestRobot extends SubsystemBase { - - // TalonFX m_intakeMotor; - // TalonFX m_armMotor; - // TalonFX m_storageMotor; - TalonFX m_outerShooter; - TalonFX m_innerShooter; - - ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0); - - - ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0); - ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0); - - ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0); - ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0); - ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0); - - VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0); - VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0); - - public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second - - public enum RobotStare { - IntakeDown, - Idle, - Shoot - } - - public RobotStare robotStare = RobotStare.Idle; - - - public TestRobot( - // TalonFX intakeMotor, - // TalonFX armMotor, - // TalonFX storageMotor, - TalonFX outerShooter, - TalonFX innerShooter - ) { - // m_intakeMotor = intakeMotor; - // m_armMotor = armMotor; - // m_storageMotor = storageMotor; - m_outerShooter = outerShooter; - m_innerShooter = innerShooter; - - // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); - // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); - // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG); - m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); - m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); - - m_outerShooter.getConfigurator().apply(SHOOTER_PID); - m_innerShooter.getConfigurator().apply(SHOOTER_PID); - - outerVelocity.Slot = 0; - innerVelocity.Slot = 0; - } - - // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - - // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - ); - public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - - ); - - public static Slot0Configs SHOOTER_PID; - - static { - SHOOTER_PID = new Slot0Configs(); - SHOOTER_PID.kV = 0.12; - // SHOOTER_PID.kP = 0.11; - // SHOOTER_PID.kI = 0.48; - // SHOOTER_PID.kD = 0.01; - SHOOTER_PID.kP = 0.3; - SHOOTER_PID.kI = 0.0; - SHOOTER_PID.kD = 0.0; - } - - - ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12); - ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11); - ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48); - ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01); - - @Override - public void periodic() { - - SHOOTER_PID = new Slot0Configs(); - SHOOTER_PID.kV = kV.get(); - SHOOTER_PID.kP = kP.get(); - SHOOTER_PID.kI = kI.get(); - SHOOTER_PID.kD = kD.get(); - - m_outerShooter.getConfigurator().apply(SHOOTER_PID); - m_innerShooter.getConfigurator().apply(SHOOTER_PID); - - - SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond)); - SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond)); - - - switch (robotStare) { - case Idle: - - // Move the arm to the up position - PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get()); - // m_armMotor.setControl(armPosition); - - - // // Stop the intake motor - // m_intakeMotor.set(0); - - // // Stop the storage motor - // m_storageMotor.set(0); - - - // Stop the outer shooter motor - m_outerShooter.set(0); - // Stop the inner shooter motor - m_innerShooter.set(0); - - break; - case IntakeDown: - // Move the arm to the down sposition - PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get()); - // m_armMotor.setControl(armPosition1); - - // Move balls into the robot because the arm is down - VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get()); - // m_intakeMotor.setControl(intakeVelocity); - - // Move balls into the main box - VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get()); - // m_storageMotor.setControl(storageVelocity); - - - // Stop the outer shooter motor - m_outerShooter.set(0); - // Stop the inner shooter motor - m_innerShooter.set(0); - - break; - case Shoot: - - // Move the arm to the up position - PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get()); - // m_armMotor.setControl(armPosition2); - - // // Stop the intake motor - // m_intakeMotor.set(0); - - // // Move the balls into the - VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get()); - // m_storageMotor.setControl(storageVelocity1); - - // outerVelocity. - // m_outerShooter.setControl(outerVelocity); - double outerRPM = outerRate.get(); - m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); - - - - // m_innerShooter.setControl(innerVelocity); - // m_innerShooter.set(innerRate.get()); - - double innerRPM = innerRate.get(); - m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); - - - // SmartDashboard.putNumber("Test", outerRate.get()); - - break; - default: - break; - } - - - - } - -} From b2d83c0123a5cd7e5d574cbe72eb5e066872a1d0 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 18:34:30 -0800 Subject: [PATCH 05/18] Update Depot Shoot.path --- src/main/deploy/pathplanner/paths/Depot Shoot.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/Depot Shoot.path b/src/main/deploy/pathplanner/paths/Depot Shoot.path index c631d45..b1445d5 100644 --- a/src/main/deploy/pathplanner/paths/Depot Shoot.path +++ b/src/main/deploy/pathplanner/paths/Depot Shoot.path @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file From 0ab81bcea1dbfb8b56977450f638173ff049eea8 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 23 Feb 2026 19:41:59 -0700 Subject: [PATCH 06/18] Adding Operator Changes --- .../frc4388/robot/subsystems/shooter/ShooterConstants.java | 2 +- src/main/java/frc4388/utility/compute/FieldPositions.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index a61f36f..3f4c63d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -21,7 +21,7 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); - public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", 35); + public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index b9c8df0..d0b438c 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -5,8 +5,8 @@ import edu.wpi.first.math.geometry.Translation2d; public class FieldPositions { // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); - public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534); + public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3); // We set the default position to one just in case it doesn't update From b3f3ae444324c4d339e4aa5dc59fe75bd3d56558 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 23 Feb 2026 23:28:25 -0700 Subject: [PATCH 07/18] Some changes --- .../java/frc4388/robot/RobotContainer.java | 1 + .../java/frc4388/robot/subsystems/Lidar.java | 20 +++++++++---------- .../robot/subsystems/swerve/SwerveDrive.java | 9 +-------- 3 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c3e51c2..49e9ec1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,6 +101,7 @@ public class RobotContainer { ); private Command LidarIntake = new SequentialCommandGroup( + // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball RobotIntakeDown, new RunCommand( () -> m_robotSwerveDrive.driveWithInputRotation( diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index d6685d9..001d241 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -64,6 +64,11 @@ public class Lidar extends SubsystemBase implements ScanListener { public Rotation2d getLatestBallAngle() { return latestBallAngleDeg; } + + public boolean outOfBounds(Translation2d closestBall){ + // Make sure robot doesn't go off the earth + return true; + } @Override @@ -237,19 +242,14 @@ public class Lidar extends SubsystemBase implements ScanListener { } if (closestBallPrior != null) { - if (closestBall.getDistance(closestBallPrior) < 0.1){ + if (closestBall.getDistance(closestBallPrior) < 0.1 && outOfBounds(closestBall)){ - Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale)); - - Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); - - // System.out.println("Drive "+ Units.metersToInches(closestBall.x) + " inches forward and " + Units.metersToInches(closestBall.y) + "inches to the right"); + // Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180); - System.out.println("!!" + latestBallAngleDeg); - } else { - Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale)); - Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); + // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 228429e..4d2c808 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -405,20 +405,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { - // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the - // swerve drive is still going: - // stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) //if no imput - // return; // don't bother doing swerve drive math and return early. - + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); - // double } public double getGyroAngle() { From 34a7ed050d2967603ebf12cd0bad0b7264aeae01 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 24 Feb 2026 17:49:26 -0700 Subject: [PATCH 08/18] Lidar and Auto testing --- .../pathplanner/autos/Tuesday Test.auto | 12 ------------ .../java/frc4388/robot/RobotContainer.java | 19 ++++++++++++------- .../robot/constants/BuildConstants.java | 10 +++++----- .../robot/subsystems/intake/Intake.java | 4 ++++ 4 files changed, 21 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Tuesday Test.auto b/src/main/deploy/pathplanner/autos/Tuesday Test.auto index 40d8425..0e1376a 100644 --- a/src/main/deploy/pathplanner/autos/Tuesday Test.auto +++ b/src/main/deploy/pathplanner/autos/Tuesday Test.auto @@ -4,18 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Quick Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } - }, { "type": "named", "data": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 49e9ec1..20a0525 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -103,24 +104,28 @@ public class RobotContainer { private Command LidarIntake = new SequentialCommandGroup( // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball RobotIntakeDown, + new WaitCommand(1), new RunCommand( - () -> m_robotSwerveDrive.driveWithInputRotation( + () -> m_robotSwerveDrive.driveWithInput( m_lidar.getClosestBall(), - m_lidar.getLatestBallAngle() + new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + false ), m_robotSwerveDrive ) - .withTimeout(10.0) + .withTimeout(5.0) .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) ); private Command RobotShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted)), new InstantCommand(() -> m_robotShooter.setShooterReady()), new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), - new WaitCommand(5), + new WaitCommand(3), new InstantCommand(()->m_robotShooter.setShooterShoot()), - new WaitCommand(10), - new InstantCommand(()->m_robotShooter.setShooterNOTShoot()) + new WaitCommand(5), + new InstantCommand(()->m_robotShooter.setShooterNOTShoot()), + new InstantCommand(() -> m_robotShooter.setShooterNotReady()) ); // private Command RobotShoot = new SequentialCommandGroup( @@ -365,7 +370,7 @@ public class RobotContainer { autoChooser.onChange((filename) -> { autoChooserUpdated = true; - // if (filename.equals("Taxi")) { + // if (filename.equals("Taxi%")) { // autoCommand = new SequentialCommandGroup( // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0, -1), diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 33fa8a8..30ca2ae 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 79; - public static final String GIT_SHA = "ff0cff819cc7280a353d7ce86999efe16661f33b"; - public static final String GIT_DATE = "2026-02-23 16:58:14 MST"; + public static final int GIT_REVISION = 86; + public static final String GIT_SHA = "b3f3ae444324c4d339e4aa5dc59fe75bd3d56558"; + public static final String GIT_DATE = "2026-02-23 23:28:25 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-23 17:39:29 MST"; - public static final long BUILD_UNIX_TIME = 1771893569353L; + public static final String BUILD_DATE = "2026-02-24 17:44:43 MST"; + public static final long BUILD_UNIX_TIME = 1771980283653L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index fc05d3a..c1a90ad 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -40,6 +40,10 @@ public class Intake extends SubsystemBase { return mode; } + public void rollerStop(){ + io.setRollerOutput(state, 0); + } + // public enum FieldZone { // // The robot should aim at the hub From 0af963284a8f94b80b5dcceb43bebdd22cca8bb9 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 24 Feb 2026 18:43:16 -0800 Subject: [PATCH 09/18] Fixing the shooter command and adding alignment --- ...hoot test.auto => Robot Reveal Night.auto} | 2 +- .../autos/Shoot and Lidar test.auto | 49 ------ .../deploy/pathplanner/paths/Quick Shoot.path | 28 +--- .../java/frc4388/robot/RobotContainer.java | 44 +++--- .../robot/commands/alignment/AutoAlign.java | 143 ++++++++++++++++++ .../robot/constants/BuildConstants.java | 10 +- .../robot/subsystems/shooter/Shooter.java | 1 + 7 files changed, 174 insertions(+), 103 deletions(-) rename src/main/deploy/pathplanner/autos/{Quick Shoot test.auto => Robot Reveal Night.auto} (92%) delete mode 100644 src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto create mode 100644 src/main/java/frc4388/robot/commands/alignment/AutoAlign.java diff --git a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto similarity index 92% rename from src/main/deploy/pathplanner/autos/Quick Shoot test.auto rename to src/main/deploy/pathplanner/autos/Robot Reveal Night.auto index 1828ee3..848e046 100644 --- a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto +++ b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto @@ -7,7 +7,7 @@ { "type": "named", "data": { - "name": "Robot Intake Down" + "name": "Robot Rev Up" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto b/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto deleted file mode 100644 index 21b7fe6..0000000 --- a/src/main/deploy/pathplanner/autos/Shoot and Lidar test.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Quick Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "DriveDepot" - } - }, - { - "type": "named", - "data": { - "name": "Lidar Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "Depot Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index 9bbf6cd..36bd07b 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -3,29 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.636650602409638, - "y": 5.051277108433735 + "x": 3.6, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 2.871710843373494, - "y": 5.215192771084337 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.3958487874465044, - "y": 4.7919115549215405 - }, - "prevControl": { - "x": 2.4481210651242846, - "y": 5.284764458740615 - }, - "nextControl": { - "x": 2.30527817403709, - "y": 3.937960057061342 + "x": 3.35, + "y": 4.0 }, "isLocked": false, "linkedName": null @@ -36,8 +20,8 @@ "y": 4.0 }, "prevControl": { - "x": 2.4025543758309, - "y": 4.230226736784185 + "x": 2.75, + "y": 4.0 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 20a0525..9aa6d00 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; @@ -98,7 +99,7 @@ public class RobotContainer { private Command RobotIntakeDown = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended)) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) ); private Command LidarIntake = new SequentialCommandGroup( @@ -117,34 +118,24 @@ public class RobotContainer { .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) ); - private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted)), - new InstantCommand(() -> m_robotShooter.setShooterReady()), - new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), - new WaitCommand(3), - new InstantCommand(()->m_robotShooter.setShooterShoot()), - new WaitCommand(5), - new InstantCommand(()->m_robotShooter.setShooterNOTShoot()), - new InstantCommand(() -> m_robotShooter.setShooterNotReady()) + private Command RobotReadyToShoot = new SequentialCommandGroup( + new InstantCommand(()-> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake), + new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter) ); - // private Command RobotShoot = new SequentialCommandGroup( - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - // new WaitCommand(5), - // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), - // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) - // ); - - // private Command RobotShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) - // ); - - // private Command RobotIntake = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) - // ); + private Command RobotShoot = new SequentialCommandGroup( + new InstantCommand(() -> + new AutoAlign(m_robotSwerveDrive, m_vision, true)), + new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), + new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), + new WaitCommand(3), + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake), + new WaitCommand(2), + new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter), + new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter) + ); + public RobotContainer() { configureButtonBindings(); @@ -163,6 +154,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); }, true); + NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot); NamedCommands.registerCommand("Robot Shoot", RobotShoot); NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown); diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java new file mode 100644 index 0000000..9ed2859 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -0,0 +1,143 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.structs.Gains; + +public class AutoAlign extends Command { + + private PID xPID = new PID(AutoConstants.XY_GAINS, 0); + private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private Pose2d targetpos; + + SwerveDrive swerveDrive; + Vision vision; + boolean waitVelocityZero; + + public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) { + this.swerveDrive = swerveDrive; + this.vision = vision; + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); + } + + public static double tagRelativeXError = -1; + private static void setTagRelativeXError(double val){ + tagRelativeXError = val; + } + + @Override + public void initialize() { + xPID.initialize(); + yPID.initialize(); + this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); + } + + double xerr; + double yerr; + double roterr; + + double xoutput; + double youtput; + double rotoutput; + + @Override + public void execute() { + xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); + yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + + roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); + + if(roterr > 180){ + roterr -= 360; + }else if(roterr < -180){ + roterr += 360; + } + + + + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); + + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + + xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + + + + Translation2d leftStick = new Translation2d( + Math.max(Math.min(-youtput, 1), -1), + Math.max(Math.min(-xoutput, 1), -1) + ); + + + + setTagRelativeXError( + new Translation2d(xerr, yerr).getAngle() + .rotateBy(targetpos.getRotation()) + .getCos()); + + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); + } + + @Override + public final boolean isFinished() { + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + ); + + if(finished) + swerveDrive.softStop(); + + return finished; + } + + + + + private class PID { + protected Gains gains; + private double output = 0; + + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(Gains gains, double tolerance) { + this.gains = gains; + } + + // Called when the command is initially scheduled. + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + public double update(double error) { + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + return output; + } + } + +} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 30ca2ae..3b38cbf 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 86; - public static final String GIT_SHA = "b3f3ae444324c4d339e4aa5dc59fe75bd3d56558"; - public static final String GIT_DATE = "2026-02-23 23:28:25 MST"; + public static final int GIT_REVISION = 87; + public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01"; + public static final String GIT_DATE = "2026-02-24 17:49:26 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-24 17:44:43 MST"; - public static final long BUILD_UNIX_TIME = 1771980283653L; + public static final String BUILD_DATE = "2026-02-24 18:48:38 MST"; + public static final long BUILD_UNIX_TIME = 1771984118729L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 31bde6b..79fbf75 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.Ready; } + public void setShooterReadyFeeder() { this.mode = ShooterMode.ReadyFeeder; } From 826a87371cf190bb101b6455e6bb177329a9bf48 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 24 Feb 2026 22:55:28 -0700 Subject: [PATCH 10/18] auto align wackiness --- .../java/frc4388/robot/RobotContainer.java | 8 +- .../robot/commands/alignment/AutoAlign.java | 82 +++++-------------- .../frc4388/robot/constants/Constants.java | 4 +- 3 files changed, 24 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9aa6d00..7c2b0cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,9 +11,7 @@ import java.io.File; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.trajectory.PathPlannerTrajectory; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; @@ -29,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; @@ -41,7 +38,6 @@ import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; -import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -124,8 +120,8 @@ public class RobotContainer { ); private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> - new AutoAlign(m_robotSwerveDrive, m_vision, true)), + // TEST NEW AUTO ALIGN + //new AutoAlign(m_robotSwerveDrive, m_vision), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 9ed2859..39164ab 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -14,101 +14,59 @@ import frc4388.utility.structs.Gains; public class AutoAlign extends Command { - private PID xPID = new PID(AutoConstants.XY_GAINS, 0); - private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; SwerveDrive swerveDrive; Vision vision; - boolean waitVelocityZero; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; - this.waitVelocityZero = waitVelocityZero && false; addRequirements(swerveDrive); } - public static double tagRelativeXError = -1; - private static void setTagRelativeXError(double val){ - tagRelativeXError = val; - } - @Override public void initialize() { - xPID.initialize(); - yPID.initialize(); + rotPID.initialize(); this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); } - double xerr; - double yerr; + double roterr; - double xoutput; - double youtput; double rotoutput; @Override public void execute() { - xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); - yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); - - roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); - if(roterr > 180){ - roterr -= 360; - }else if(roterr < -180){ - roterr += 360; - } - + Pose2d robotPose = vision.getPose2d(); + if (robotPose == null) return; + + double dx = targetpos.getX() - robotPose.getX(); + double dy = targetpos.getY() - robotPose.getY(); + + Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx)); + + roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); + if (roterr > 180) roterr -= 360; + if (roterr < -180) roterr += 360; - SmartDashboard.putNumber("PID X Error", xerr); - SmartDashboard.putNumber("PID Y Error", yerr); SmartDashboard.putNumber("PID Rot Error", roterr); - xoutput = xPID.update(xerr); - youtput = yPID.update(yerr); - - xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - - - - Translation2d leftStick = new Translation2d( - Math.max(Math.min(-youtput, 1), -1), - Math.max(Math.min(-xoutput, 1), -1) - ); - - - - setTagRelativeXError( - new Translation2d(xerr, yerr).getAngle() - .rotateBy(targetpos.getRotation()) - .getCos()); - - swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); - } + swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); + } @Override public final boolean isFinished() { - boolean finished = ( - (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) - ); - - if(finished) + boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; + if (finished) { swerveDrive.softStop(); - + } return finished; } - - - private class PID { protected Gains gains; private double output = 0; diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 20e0a13..96e667e 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -47,7 +47,7 @@ public final class Constants { public static final class AutoConstants { // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); - public static final Gains XY_GAINS = new Gains(8,0,0.0); + public static final Gains ROT_GAINS = new Gains(8,0,0.0); // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); @@ -61,7 +61,7 @@ public final class Constants { public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 5; // Degrees + public static final double ROT_TOLERANCE = 10; // Degrees public static final double MIN_XY_PID_OUTPUT = 0.0; public static final double MIN_ROT_PID_OUTPUT = 0.0; From 3ac4ac230f17f2a89e5bfd8620c09d7e999aaad0 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 24 Feb 2026 23:02:08 -0700 Subject: [PATCH 11/18] Paths --- .../{Tuesday Test.auto => Lidar Intake.auto} | 0 .../autos/Test- Robot Align Path.auto | 37 +++++++++++++ .../deploy/pathplanner/paths/DriveDepot.path | 54 ------------------- ...pot Shoot.path => Test - Robot Align.path} | 14 ++--- 4 files changed, 44 insertions(+), 61 deletions(-) rename src/main/deploy/pathplanner/autos/{Tuesday Test.auto => Lidar Intake.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto delete mode 100644 src/main/deploy/pathplanner/paths/DriveDepot.path rename src/main/deploy/pathplanner/paths/{Depot Shoot.path => Test - Robot Align.path} (85%) diff --git a/src/main/deploy/pathplanner/autos/Tuesday Test.auto b/src/main/deploy/pathplanner/autos/Lidar Intake.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Tuesday Test.auto rename to src/main/deploy/pathplanner/autos/Lidar Intake.auto diff --git a/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto b/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto new file mode 100644 index 0000000..6c7bc18 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "Quick Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Test - Robot Align" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DriveDepot.path b/src/main/deploy/pathplanner/paths/DriveDepot.path deleted file mode 100644 index f98a2c2..0000000 --- a/src/main/deploy/pathplanner/paths/DriveDepot.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.5, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.5000000000000004, - "y": 7.854276578779859 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.47854216867347843, - "y": 6.9745542168703265 - }, - "prevControl": { - "x": 0.34740963855421675, - "y": 7.44444578313253 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot Shoot.path b/src/main/deploy/pathplanner/paths/Test - Robot Align.path similarity index 85% rename from src/main/deploy/pathplanner/paths/Depot Shoot.path rename to src/main/deploy/pathplanner/paths/Test - Robot Align.path index b1445d5..0b4c861 100644 --- a/src/main/deploy/pathplanner/paths/Depot Shoot.path +++ b/src/main/deploy/pathplanner/paths/Test - Robot Align.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4401807228915664, - "y": 5.608590361445783 + "x": 2.5, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 1.4401807228915664, - "y": 4.710738110733342 + "x": 2.526567580029336, + "y": 4.248584319077823 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 2.5, - "y": 4.0 + "y": 4.7 }, "prevControl": { - "x": 2.5, - "y": 4.270156342259512 + "x": 2.5175324328959037, + "y": 4.450615530161258 }, "nextControl": null, "isLocked": false, From b3b32b00fed827f8efe77d0f6710d36222a209ac Mon Sep 17 00:00:00 2001 From: Shikhar Date: Wed, 25 Feb 2026 17:17:52 -0700 Subject: [PATCH 12/18] chaning auto align --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- .../java/frc4388/robot/commands/alignment/AutoAlign.java | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7c2b0cc..48e6982 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -12,6 +12,7 @@ import java.io.File; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; @@ -121,7 +122,7 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0))), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 39164ab..d7c0687 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -9,7 +9,6 @@ import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; -import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.structs.Gains; public class AutoAlign extends Command { @@ -20,16 +19,17 @@ public class AutoAlign extends Command { SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { this.swerveDrive = swerveDrive; this.vision = vision; + this.targetpos = targetpos; addRequirements(swerveDrive); } @Override public void initialize() { rotPID.initialize(); - this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); + //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); } From 4ca55e0225e0102c70765482219b7eddc01eb2d4 Mon Sep 17 00:00:00 2001 From: SHikhar Date: Wed, 25 Feb 2026 17:45:22 -0700 Subject: [PATCH 13/18] Lidar Test --- .../java/frc4388/robot/RobotContainer.java | 25 +++++++++++-------- .../robot/commands/alignment/AutoAlign.java | 11 ++++---- .../robot/constants/BuildConstants.java | 10 ++++---- 3 files changed, 24 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 48e6982..e0f4b06 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -103,16 +103,19 @@ public class RobotContainer { // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball RobotIntakeDown, new WaitCommand(1), - new RunCommand( - () -> m_robotSwerveDrive.driveWithInput( - m_lidar.getClosestBall(), - new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - false - ), - m_robotSwerveDrive - ) - .withTimeout(5.0) - .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true) + + // new RunCommand( + // () -> m_robotSwerveDrive.driveWithInput( + // m_lidar.getClosestBall(), + // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + // false + // ), + // m_robotSwerveDrive + // ) + // .withTimeout(5.0) + // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) ); private Command RobotReadyToShoot = new SequentialCommandGroup( @@ -122,7 +125,7 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN - new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0))), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index d7c0687..79334f6 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -15,14 +15,15 @@ public class AutoAlign extends Command { private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; - + private boolean isLidar; SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) { this.swerveDrive = swerveDrive; this.vision = vision; this.targetpos = targetpos; + this.isLidar = isLidar; addRequirements(swerveDrive); } @@ -39,8 +40,6 @@ public class AutoAlign extends Command { @Override public void execute() { - - Pose2d robotPose = vision.getPose2d(); if (robotPose == null) return; @@ -54,9 +53,9 @@ public class AutoAlign extends Command { if (roterr < -180) roterr += 360; SmartDashboard.putNumber("PID Rot Error", roterr); - + System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); - } + } @Override public final boolean isFinished() { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 3b38cbf..77101ba 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 87; - public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01"; - public static final String GIT_DATE = "2026-02-24 17:49:26 MST"; + public static final int GIT_REVISION = 91; + public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac"; + public static final String GIT_DATE = "2026-02-25 17:17:52 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-24 18:48:38 MST"; - public static final long BUILD_UNIX_TIME = 1771984118729L; + public static final String BUILD_DATE = "2026-02-25 17:44:49 MST"; + public static final long BUILD_UNIX_TIME = 1772066689466L; public static final int DIRTY = 1; private BuildConstants(){} From 0173c9599613c0968ad2ef8e9f81694fe4bb3882 Mon Sep 17 00:00:00 2001 From: SHikhar Date: Wed, 25 Feb 2026 17:50:47 -0700 Subject: [PATCH 14/18] changing for the gyro --- .../java/frc4388/robot/commands/alignment/AutoAlign.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 79334f6..1f85d2b 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -40,13 +40,19 @@ public class AutoAlign extends Command { @Override public void execute() { + Rotation2d desiredHeading; Pose2d robotPose = vision.getPose2d(); if (robotPose == null) return; double dx = targetpos.getX() - robotPose.getX(); double dy = targetpos.getY() - robotPose.getY(); - Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx)); + if (!isLidar){ + desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI); + }else{ + desiredHeading = new Rotation2d(Math.atan2(dy, dx)); + } + roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); if (roterr > 180) roterr -= 360; From ff706b70ec5949fd30b54375d22a2cf4a5295bc0 Mon Sep 17 00:00:00 2001 From: SHikhar Date: Wed, 25 Feb 2026 17:52:47 -0700 Subject: [PATCH 15/18] freaky lidar --- src/main/java/frc4388/robot/commands/alignment/AutoAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 1f85d2b..82121d7 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -50,7 +50,7 @@ public class AutoAlign extends Command { if (!isLidar){ desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI); }else{ - desiredHeading = new Rotation2d(Math.atan2(dy, dx)); + desiredHeading = new Rotation2d(Math.atan2(dx, dy)); } From 444954fa98d7bd99fc77ab3c12ba6678a1e83aaf Mon Sep 17 00:00:00 2001 From: SHikhar Date: Wed, 25 Feb 2026 18:12:40 -0700 Subject: [PATCH 16/18] no auto align on robot shoot yet --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e0f4b06..bda7d7f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN - new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), + //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), From 3745cc2b1869e5850c93507277539c7cfed606c1 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Wed, 25 Feb 2026 18:59:11 -0700 Subject: [PATCH 17/18] Update Lidar.java --- .../java/frc4388/robot/subsystems/Lidar.java | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 001d241..fdbf2a6 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -31,7 +31,7 @@ public class Lidar extends SubsystemBase implements ScanListener { // private final Spark m_motor; private final RPLidarA1 lidar; - private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.7); + private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.2); static { @@ -85,7 +85,7 @@ public class Lidar extends SubsystemBase implements ScanListener { private static final double RADIUS_X_COEFF = Units.inchesToMeters(0); private static final double RADIUS_Y_COEFF = Units.inchesToMeters(0); private static final double RADIUS_OFFSET = Units.inchesToMeters(3); - private static final double RADIUS_TOLERANCE = Units.inchesToMeters(0.8); + private static final double RADIUS_TOLERANCE = Units.inchesToMeters(3); private static boolean radiusInTolerance(double x, double y, double radius) { double rad_at_position = RADIUS_X_COEFF*x + RADIUS_Y_COEFF*y + RADIUS_OFFSET; @@ -122,20 +122,24 @@ public class Lidar extends SubsystemBase implements ScanListener { System.out.println("SCAN: " + scan.size()); - double scale = 0.006; + double scale = 0.009; List circlePoints = new ArrayList<>(); - Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3); + // Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3); for(PolarPoint point_polar : scan) { + if(!(point_polar.angle < 30 || point_polar.angle > 330)) { + continue; + } + double ang_rad = Math.toRadians(point_polar.angle); double x = point_polar.distance * Math.cos(ang_rad); double y = point_polar.distance * Math.sin(ang_rad); // Point point_xy = new Point((WIDTH/2) + (x/scale), (HEIGHT/2) + (y/scale)); Point point_xy = new Point(x, y); - + if( Math.abs(last_ang - point_polar.angle) > ANG_MAX_GAP || Math.abs(last_dist - point_polar.distance) > DIST_MAX_GAP @@ -211,15 +215,15 @@ public class Lidar extends SubsystemBase implements ScanListener { last_ang = point_polar.angle; last_dist = point_polar.distance; - Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale)); - Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127)); + // Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale)); + // Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127)); } - for(Translation2d circle : circlePoints) { - Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale)); - Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255)); - // System.out.println(circle.x + " - " + circle.y); - } + // for(Translation2d circle : circlePoints) { + // Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255)); + // // System.out.println(circle.x + " - " + circle.y); + // } @@ -248,7 +252,7 @@ public class Lidar extends SubsystemBase implements ScanListener { // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180); } else { - // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale)); + // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getY() / scale)); // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); } } @@ -257,12 +261,12 @@ public class Lidar extends SubsystemBase implements ScanListener { - Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1); + // Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1); // System.o - showWindow(mat); + // showWindow(mat); } private static void showWindow(Mat img) { From f9174c23cd43f702a029065d59b30f9a1052c55a Mon Sep 17 00:00:00 2001 From: Shikhar Date: Wed, 25 Feb 2026 20:21:47 -0700 Subject: [PATCH 18/18] Lidar Align --- .../pathplanner/autos/Robot Reveal Night.auto | 6 ++ .../java/frc4388/robot/RobotContainer.java | 17 ++++-- .../robot/commands/alignment/AutoAlign.java | 55 ++++++++++++------- .../robot/constants/BuildConstants.java | 10 ++-- .../subsystems/intake/IntakeConstants.java | 4 +- 5 files changed, 58 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto index 848e046..ddb4f8a 100644 --- a/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto +++ b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto @@ -10,6 +10,12 @@ "name": "Robot Rev Up" } }, + { + "type": "named", + "data": { + "name": "Intake Retracted" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bda7d7f..56a442f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,10 +101,10 @@ public class RobotContainer { private Command LidarIntake = new SequentialCommandGroup( // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball - RobotIntakeDown, - new WaitCommand(1), - new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true) + // RobotIntakeDown, + // new WaitCommand(1), + // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) // new RunCommand( // () -> m_robotSwerveDrive.driveWithInput( @@ -123,14 +123,18 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter) ); + private Command IntakeRetracted = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + ); + private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), - new WaitCommand(3), - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake), new WaitCommand(2), + IntakeRetracted, + new WaitCommand(3), new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter), new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter) ); @@ -155,6 +159,7 @@ public class RobotContainer { }, true); NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot); + NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); NamedCommands.registerCommand("Robot Shoot", RobotShoot); NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown); diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 82121d7..770280d 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; @@ -14,15 +15,19 @@ import frc4388.utility.structs.Gains; public class AutoAlign extends Command { private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); - private Pose2d targetpos; + private Lidar lidar; private boolean isLidar; + + private Pose2d targetpos; + private double targetRotation; + SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) { this.swerveDrive = swerveDrive; this.vision = vision; - this.targetpos = targetpos; + this.lidar = lidar; this.isLidar = isLidar; addRequirements(swerveDrive); } @@ -30,6 +35,7 @@ public class AutoAlign extends Command { @Override public void initialize() { rotPID.initialize(); + this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees(); //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); } @@ -40,36 +46,43 @@ public class AutoAlign extends Command { @Override public void execute() { - Rotation2d desiredHeading; - Pose2d robotPose = vision.getPose2d(); - if (robotPose == null) return; + double desiredHeading; + // Pose2d robotPose = vision.getPose2d(); + targetpos = new Pose2d(lidar.getClosestBall(), new Rotation2d(0)); + // if (robotPose == null) return; + if (targetpos == null) return; + if (targetpos.getTranslation() == null) return; - double dx = targetpos.getX() - robotPose.getX(); - double dy = targetpos.getY() - robotPose.getY(); + + double dx = targetpos.getX();// - robotPose.getX(); + double dy = targetpos.getY();// - robotPose.getY(); if (!isLidar){ - desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI); + desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180; }else{ - desiredHeading = new Rotation2d(Math.atan2(dx, dy)); + desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180; } - roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); - if (roterr > 180) roterr -= 360; - if (roterr < -180) roterr += 360; + targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading; - SmartDashboard.putNumber("PID Rot Error", roterr); - System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); - swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); + // roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); + // if (roterr > 180) roterr -= 360; + // if (roterr < -180) roterr += 360; + + SmartDashboard.putNumber("Target Rotation!", targetRotation); + // System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); + swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(targetRotation)); } @Override public final boolean isFinished() { - boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; - if (finished) { - swerveDrive.softStop(); - } - return finished; + // boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; + // if (finished) { + // swerveDrive.softStop(); + // } + // return finished; + return false; } private class PID { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 77101ba..4298429 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 91; - public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac"; - public static final String GIT_DATE = "2026-02-25 17:17:52 MST"; + public static final int GIT_REVISION = 96; + public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1"; + public static final String GIT_DATE = "2026-02-25 18:59:11 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-25 17:44:49 MST"; - public static final long BUILD_UNIX_TIME = 1772066689466L; + public static final String BUILD_DATE = "2026-02-25 20:19:51 MST"; + public static final long BUILD_UNIX_TIME = 1772075991136L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index b46648a..5a9e1c2 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -45,13 +45,13 @@ public class IntakeConstants { public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(0.1) + .withKP(0.03) .withKI(0.0) .withKD(0.0); - public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.03); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);