Add States to drive

build.gradle

@@ -101,7 +101,7 @@ dependencies {
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 
-    def coppercoreVersion = "2025.1.7" // mavenLocal version
+    def coppercoreVersion = "2025.1.8" // mavenLocal version
 
     implementation "io.github.team401.coppercore:controls:$coppercoreVersion"
     implementation "io.github.team401.coppercore:geometry:$coppercoreVersion"

simgui-ds.json

@@ -90,11 +90,9 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
-      "guid": "030000006d04000014c2000000000000"
-    },
-    {
-      "guid": "030000006d04000014c2000000000000"
+      "guid": "030000004f0400000ab1000000050000"
     }
   ]
 }

src/main/deploy/constants/RedFieldLocations.json

@@ -1,7 +1,7 @@
 {
   "redReefCenterTranslation": {
-    "x": 176.75,
-    "y": 158.5
+    "x": 12.5,
+    "y": 4.0259
   },
   "redReefCenterRotation": {
     "radians": 0.0

src/main/deploy/constants/test_drivebase/DrivetrainConstants.json

@@ -30,7 +30,7 @@
   "driveAlongTrackMultiplier": -1,
   "driveAlongTrackOffset": 0.25,
   "driveCrossTrackFrontRightOffset": 0.0,
-  "otfPoseDistanceLimit": 0.1,
+  "otfPoseDistanceLimit": 0.2,
   "kSteerClosedLoopOutput": "Voltage",
   "kDriveClosedLoopOutput": "TorqueCurrentFOC",
   "kDriveMotorType": "TalonFX_Integrated",
@@ -330,7 +330,7 @@
   "kInvertRightSide": true,
   "kPigeonId": 20,
   "OTFMaxLinearVelocity": 3.5,
-  "OTFMaxLinearAccel": 0.5,
+  "OTFMaxLinearAccel": 2.0,
   "OTFMaxAngularVelocity": 3.141592653589793,
   "OTFMaxAngularAccel": 1.5707963267948966,
   "DrivetrainConstants": {

src/main/java/frc/robot/BuildConstants.java

@@ -3,14 +3,14 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2025-Robot-Code-2";
+  public static final String MAVEN_NAME = "2025-Robot-Code";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 46;
-  public static final String GIT_SHA = "25f2cfcfd7a62559b6af5fc71c2258639513996c";
-  public static final String GIT_DATE = "2025-02-05 19:00:34 EST";
-  public static final String GIT_BRANCH = "add-lock-on-reef";
-  public static final String BUILD_DATE = "2025-02-05 19:11:03 EST";
-  public static final long BUILD_UNIX_TIME = 1738800663170L;
+  public static final int GIT_REVISION = 41;
+  public static final String GIT_SHA = "6014babca388b45cfb9884e60358fcf30f37bc46";
+  public static final String GIT_DATE = "2025-02-06 19:36:46 EST";
+  public static final String GIT_BRANCH = "drive-state";
+  public static final String BUILD_DATE = "2025-02-06 19:49:52 EST";
+  public static final long BUILD_UNIX_TIME = 1738889392945L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/InitBindings.java

@@ -13,6 +13,7 @@
 import frc.robot.constants.JsonConstants;
 import frc.robot.constants.OperatorConstants;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.Drive.DriveTrigger;
 
 public final class InitBindings {
   // Controller
@@ -44,15 +45,15 @@ public static void initDriveBindings(Drive drive) {
         .onTrue(
             new InstantCommand(
                 () -> {
-                  drive.setAutoAlignment(true);
+                  drive.fireTrigger(DriveTrigger.BeginAutoAlignment);
                 },
                 drive));
     rightJoystick
         .button(1)
         .onFalse(
             new InstantCommand(
                 () -> {
-                  drive.setAutoAlignment(false);
+                  drive.fireTrigger(DriveTrigger.CancelAutoAlignment);
                 },
                 drive));
 
@@ -101,5 +102,24 @@ public static void initDriveBindings(Drive drive) {
                       new Pose2d(
                           Meters.of(14.350), Meters.of(4.0), new Rotation2d(Degrees.of(180))));
                 }));
+    rightJoystick
+        .top()
+        .onTrue(
+            new InstantCommand(
+                () -> {
+                  drive.setGoToIntake(true);
+                  drive.fireTrigger(DriveTrigger.BeginAutoAlignment);
+                },
+                drive));
+
+    rightJoystick
+        .top()
+        .onFalse(
+            new InstantCommand(
+                () -> {
+                  drive.setGoToIntake(false);
+                  drive.fireTrigger(DriveTrigger.CancelAutoAlignment);
+                },
+                drive));
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -131,7 +131,7 @@ public void testPeriodic() {
       scoringSubsystem.testPeriodic();
     }
     if (FeatureFlags.synced.getObject().runDrive) {
-      drive.testPeriodic();
+      // TODO: run lineup state test periodic
     }
   }
 

src/main/java/frc/robot/constants/JsonConstants.java

@@ -22,7 +22,6 @@ public static void loadConstants() {
     ScoringSetpoints.synced.loadData();
     RedFieldLocations.synced.loadData();
     BlueFieldLocations.synced.loadData();
-    DrivetrainConstants.synced.loadData();
     VisionConstants.synced.loadData();
 
     elevatorConstants = ElevatorConstants.synced.getObject();

src/main/java/frc/robot/constants/field/RedFieldLocations.java

@@ -17,7 +17,8 @@ public class RedFieldLocations {
               .toString(),
           new JSONSyncConfigBuilder().setPrettyPrinting(true).build());
 
-  public Translation2d redReefCenterTranslation = new Translation2d(176.75, (158.5));
+  public Translation2d redReefCenterTranslation =
+      new Translation2d(Units.inchesToMeters(176.75), Units.inchesToMeters(158.5));
   public Rotation2d redReefCenterRotation = new Rotation2d();
 
   public Translation2d redReef01Translation = new Translation2d(11.5, 4.1);

src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java

@@ -142,7 +142,7 @@ public class DrivetrainConstants {
 
   // OTF Max speed / accel
   public final Double OTFMaxLinearVelocity = 3.5;
-  public final Double OTFMaxLinearAccel = 0.5;
+  public final Double OTFMaxLinearAccel = 2.0;
   public final Double OTFMaxAngularVelocity = Math.PI;
   public final Double OTFMaxAngularAccel = Math.PI / 2;