Work in progress

src/main/java/frc4388/robot/subsystems/led/LED.java

@@ -5,7 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.led;
 
 import java.util.concurrent.TimeUnit;
 

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -15,8 +15,8 @@ import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants;
-import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.utility.compute.FieldPositions;
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -377,6 +377,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         driveFieldAngle(leftStick, ang);
     }
 
+    public Pose2d getCurrentPose(){
+        return state.currentPose;
+    }
+
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
         leftStick = leftStick.rotateBy(heading);
 
@@ -515,6 +519,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         gear_index = i;
     }
 
+
     public void setPercentOutput(double speed) {
         speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
         gear_index = -1;

src/main/java/frc4388/robot/subsystems/vision/Lidar.java

@@ -1,4 +1,4 @@
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.vision;
 
 import java.util.ArrayList;
 import java.util.LinkedList;
@@ -20,8 +20,8 @@ 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.robot.subsystems.vision.RPLidarA1.PolarPoint;
+import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener;
 import frc4388.utility.configurable.ConfigurableDouble;
 import frc4388.utility.status.FaultA1M8;
 

src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java

@@ -1,4 +1,4 @@
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.vision;
 
 import com.fazecast.jSerialComm.SerialPort;
 import edu.wpi.first.wpilibj.DriverStation;