Fueldetection

.vscode/launch.json

@@ -19,25 +19,3 @@
     }
   ]
 }
-
-{
-  // Use IntelliSense to learn about possible attributes.
-  // Hover to view descriptions of existing attributes.
-  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
-  "version": "0.2.0",
-  "configurations": [
-
-    {
-      "type": "wpilib",
-      "name": "WPILib Desktop Debug",
-      "request": "launch",
-      "desktop": true,
-    },
-    {
-      "type": "wpilib",
-      "name": "WPILib roboRIO Debug",
-      "request": "launch",
-      "desktop": false,
-    }
-  ]
-}

.vscode/settings.json

@@ -55,7 +55,8 @@
     "javax.smartcardio.*",
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
-    "edu.wpi.first.math.**.struct.*",
+    "edu.wpi.first.math.**.stru
+ct.*",
   ],
   "java.dependency.enableDependencyCheckup": false
-}
+}

src/main/java/frc/robot/Constants.java

@@ -3,13 +3,6 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.path.PathConstraints;
 
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.subsystems.drive.DriveConstants;
 
@@ -29,83 +22,8 @@ public static final class Controllers {
 	}
 
 	public static final class Odometry {
-		public static final int OBSERVATION_BUFFER_SIZE = 50;
-		public static final Matrix<N2, N1> STATE_STD_DEVS = VecBuilder.fill(Math.pow(0.05, 1), Math.pow(0.05, 1)); // drive
-		public static final Matrix<N2, N1> LOCAL_MEASUREMENT_STD_DEVS = VecBuilder.fill(
-				Math.pow(0.02, 1), // vision
-				Math.pow(0.02, 1));
 	}
-	public static final class Limelight { //TODO: this must be tuned to specific robot
-		public static enum VisionDeviceConstants {
-			// L_CONSTANTS (
-			// 	"limelight-left",
-			// 	new Transform2d(
-			// 		new Translation2d(Units.Inches.of(10.5), Units.Inches.of(1.23)),
-			// 		Rotation2d.fromDegrees(-90)),
-			// 	0, 1600, 1200),
-
-			// R_CONSTANTS (
-			// 	"limelight-right",
-			// 	new Transform2d(
-            //         new Translation2d(Units.Inches.of(10.78), Units.Inches.of(2)),
-            //         Rotation2d.fromDegrees(90)),
-			// 	0, 1600, 1200),
-
-			// F_CONSTANTS (
-			// 	"limelight-front",
-			// 	new Transform2d(
-			// 		new Translation2d(Units.Inches.of(11.11), Units.Inches.of(4.28)),
-			// 		Rotation2d.fromDegrees(0)),
-			// 	0, 1600, 1200),
-
-			// B_CONSTANTS (
-			// 	"limelight-back",
-			// 	new Transform2d(
-			// 		new Translation2d(Units.Inches.of(0), Units.Inches.of(-0.96)),
-			// 		Rotation2d.fromDegrees(180)),
-			// 	0, 1600, 1200);
-
-			// R_CONSTANTS (
-			// 	"right",  
-			// 	new edu.wpi.first.math.geometry.Transform3d(
-			// 		new Translation3d(0.267, 0.0312, 0.2791),
-			//		new Rotation3d(TAU/4, -TAU/4, 0)),
-			//	0, 1280, 800),
-
-			FR_CONSTANTS (
-				"frontr",
-				new edu.wpi.first.math.geometry.Transform3d(
-					new Translation3d(0.2822, 0.1087, 0.1984),
-					new Rotation3d(0.5 * TAU, 14.0 * TAU / 360.0, -26.0 * TAU/360.0)),
-				1, 1280, 800),
-
-			FL_CONSTANTS (
-				"frontl",
-				new edu.wpi.first.math.geometry.Transform3d(
-					new Translation3d(0.2822, -0.1087, 0.1984),
-					new Rotation3d(0.5 * TAU, 14.0 * TAU / 360.0, 26.0 * TAU/360.0)),
-				2, 1280, 800);
-
-			public final String tableName;
-			public final Transform3d robotToCamera;
-			public final int cameraId;
-			public final int cameraResolutionWidth;
-			public final int cameraResolutionHeight;
-			private VisionDeviceConstants(
-				String tableName, 
-				Transform3d robotToCamera,
-				int cameraId, 
-				int cameraResolutionWidth,
-				int cameraResolutionHeight
-			) {
-				this.tableName = tableName;
-				this.robotToCamera = robotToCamera;
-				this.cameraId = cameraId;
-				this.cameraResolutionWidth = cameraResolutionWidth;
-				this.cameraResolutionHeight = cameraResolutionHeight;
-			}
-		}
-    }
+
 
 	public static final class Pathplanner {
 		public static RobotConfig config;

src/main/java/frc/robot/ControlsMapping.java

@@ -7,20 +7,26 @@
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType;
+import frc.robot.subsystems.vision.VisionDeviceManager;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 public class ControlsMapping {
 	public static void mapTeleopCommand() {
-		Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand()));
+		Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl()));
 		// run sysID functions
 		Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER);
 
 		controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d()));
-		controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true));
-		controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false));
-		controller.x().whileTrue(Drive.getInstance().autopilotAlign(true));
-		controller.y().whileTrue(Drive.getInstance().autopilotAlign(false));
+
+		controller.b().whileTrue(Drive.getInstance().headingLockToPose(DriveConstants.FieldPoses.TAG.pose));
+		controller.x().onTrue(Drive.getInstance().pathFindToThisRandomPlaceIdk());
+		// controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true));
+		// controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false));
+		// controller.x().whileTrue(Drive.getInstance().autopilotAlign(true));
+		// controller.y().whileTrue(Drive.getInstance().autopilotAlign(false));
+		controller.y().onTrue(VisionDeviceManager.getInstance().bootUp());
 	}
 
 	public static void mapSysId() {

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

@@ -42,12 +42,12 @@ public class Robot extends TimedRobot {
 	 * initialization code.
 	 */
 	public Robot() {
-		if (Robot.isReal()) {
-			VisionDeviceManager.getInstance();
-		}
+		VisionDeviceManager.getInstance();
+
 		Drive.getInstance();
 		TelemetryManager.getInstance();
-		FollowPathCommand.warmupCommand().schedule();
+		commandScheduler.schedule(FollowPathCommand.warmupCommand());
+		commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp());
 		autoChooser = new AutoSelector();
 
 		//robot data loggers 
@@ -94,7 +94,7 @@ public void disabledPeriodic() {
 	public void autonomousInit() {
 		autoCommand = autoChooser.getAuto();
 		if (autoCommand != null) {
-			autoCommand.schedule();
+			commandScheduler.schedule(autoCommand);
 		} else {
 			DriverStation.reportWarning("Tried to schedule a null auto", false);
 		}
@@ -116,7 +116,7 @@ public void teleopInit() {
 		if (autoCommand != null) {
 			autoCommand.cancel();
 		}
-		Drive.getInstance().setDefaultCommand(Drive.getInstance().teleopCommand());
+		Drive.getInstance().setDefaultCommand(Drive.getInstance().openLoopControl());
 
 		ControlsMapping.mapTeleopCommand();
 	}

src/main/java/frc/robot/lib/control/ProfiledPIDVController.java

@@ -55,17 +55,17 @@ public ProfiledPIDVController setMeasurement(double position, double speed) {
 	}
 
 	public double getOutput() {
-		// if (controller.isContinuous) {
-		// 	double errorBound = (controller.maxRange - controller.minRange) / 2.0;
+		if (controller.isContinuous) {
+			double errorBound = (controller.maxRange - controller.minRange) / 2.0;
 
-		// 	double goalDelta =
-		// 		MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound);
-		// 	double setpointDelta =
-		// 		MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound);
+			double goalDelta =
+				MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound);
+			double setpointDelta =
+				MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound);
 
-		// 	goal.position = goalDelta + controller.positionMeasurement;
-		// 	setpoint.position = setpointDelta + controller.positionMeasurement;
-		// }
+			goal.position = goalDelta + controller.positionMeasurement;
+			setpoint.position = setpointDelta + controller.positionMeasurement;
+		}
 
 		// Advance profile by one timestep
 		setpoint = profile.calculate(Constants.DT, setpoint, goal);