fix crashing works

Co-Authored-By: Farhan Jamil <71662961+FarhanJ2@users.noreply.github.com>
Co-Authored-By: JacobMarzo <152223032+JacobMarzo@users.noreply.github.com>

Author: 3 people

src/main/java/org/steelhawks/Constants.java

@@ -41,7 +41,7 @@ public enum RobotType {
     }
 
     // Change this based on what robot is being used.
-    private static final RobotType ROBOT = RobotType.SIMBOT;
+    private static final RobotType ROBOT = RobotType.ALPHABOT;
 
     /**
      * The robot type.

src/main/java/org/steelhawks/RobotContainer.java

@@ -111,6 +111,7 @@ public RobotContainer() {
                             new ModuleIOTalonFX(TunerConstantsAlpha.FrontRight),
                             new ModuleIOTalonFX(TunerConstantsAlpha.BackLeft),
                             new ModuleIOTalonFX(TunerConstantsAlpha.BackRight));
+                    s_Elevator = new Elevator(new ElevatorIO() {});
                     s_Vision =
                         new Vision(
                             s_Swerve::accept,
@@ -240,7 +241,7 @@ public RobotContainer() {
             }
         }
         new Alert("Use Vision is Off", AlertType.kWarning).set(!Toggles.Vision.visionEnabled.get());
-        Autos.init();
+        // Autos.init();
 
         if (Constants.getRobot() != RobotType.ALPHABOT) {
             checkIfDevicesConnected();

src/main/java/org/steelhawks/subsystems/elevator/ElevatorConstants.java

@@ -46,27 +46,28 @@ public Rotation2d getAngle() {
     public static final double SPROCKET_RAD = // the driving drum
         Units.inchesToMeters(1.888);
 
-    public static final Double MAX_VELOCITY_ROT_PER_SEC = Constants.omega(25.0, 10.0);
-    public static final Double MAX_ACCELERATION_ROT_PER_SEC_2 = Constants.omega(46.0, 20.0);
+    public static final Double MAX_VELOCITY_ROT_PER_SEC = Constants.value(0.0,10.0, 25.0);
+    public static final Double MAX_ACCELERATION_ROT_PER_SEC_2 = Constants.value(0.0,20.0, 46.0);
     public static final Double MANUAL_ELEVATOR_INCREMENT = 0.65;
-    public static final Double MANUAL_ELEVATOR_RAMP_RATE = Constants.omega(0.6 * MANUAL_ELEVATOR_INCREMENT, 0.8 * MANUAL_ELEVATOR_INCREMENT);
+    public static final Double MANUAL_ELEVATOR_RAMP_RATE = Constants.value(0.0, 0.0,0.6 * MANUAL_ELEVATOR_INCREMENT, 0.8 * MANUAL_ELEVATOR_INCREMENT);
 
-    public static final LoggedTunableNumber KP = new LoggedTunableNumber("Elevator/kP", Constants.omega(500.0, 6.0));
+    public static final LoggedTunableNumber KP = new LoggedTunableNumber("Elevator/kP", Constants.value(0.0, 0.0, 500.0, 6.0));
     public static final double KI = 0.0;
-    public static final LoggedTunableNumber KD = new LoggedTunableNumber("Elevator/kD", Constants.omega(40.0, 0.4));
+    public static final LoggedTunableNumber KD = new LoggedTunableNumber("Elevator/kD", Constants.value(0.0, 0.0,40.0, 0.4));
 
     public static final double ELEVATOR_DISTANCE_INTERPOLATOR_MAX = 0.6;
     public static final double SHIMMY_VOLTS = -5.0;
     public static final double HOMING_VOLTS = -3.0;
 
     public static final Double[] kS = {
-        Constants.omega(1.0, 0.0),
+        Constants.value(0.0, 0.0, 1.0),
         0.0,
         0.0
     };
 
     public static final Double[] kV = {
-        Constants.omega(
+        Constants.value(
+            0.0,
         0.0, 0.0),
         0.0,
         0.0
@@ -75,7 +76,7 @@ public Rotation2d getAngle() {
     };
 
     public static final Double[] kG = {
-        Constants.omega(6.0, 0.0),
+        Constants.value(0.0, 0.0, 6.0),
         0.0,
         0.0
     };

src/main/java/org/steelhawks/subsystems/vision/QuestNavImpl.java

@@ -100,7 +100,7 @@ public void periodic() {
                     || Constants.loggedValue("PhysicallyRealisticMotion", !isPhysicallyFeasible(robotPose, frame.dataTimestamp()))
                     || Constants.loggedValue("QuestTracking", !nav.isTracking())
                     || Constants.loggedValue("QuestConnected", !nav.isConnected());
-                Logger.recordOutput("QuestNav/UnfilteredPose", robotPose);
+                Logger.recordOutput("QuestNav/UnfilteredPose3d", frame.questPose3d());
                 if (rejectPose) {
                     allQuestPosesRejected.add(robotPose);
                 } else {

src/main/java/org/steelhawks/subsystems/vision/Vision.java

@@ -263,7 +263,8 @@ public void periodic() {
         if (questNav != null) {
             // make it so that in debug mode you can automatically reset pose from vision when disabled/ or make it a toggle
             if (DriverStation.isDisabled() && Robot.isFirstRun() && Constants.loggedValue("RobotPosesEmpty", !allRobotPoses.isEmpty())) {
-                questNav.setPose(allRobotPosesAccepted.get(0).toPose2d());
+//                questNav.setPose(allRobotPosesAccepted.get(0).toPose2d());
+                questNav.setPose(new Pose2d());
             }
             questNav.periodic();
             LoopTimeUtil.record("QuestNav");