Rewrite LOL

.gitignore

@@ -30,3 +30,5 @@ simgui-ds.json
 simgui-window.json
 simgui.json
 networktables.json
+bin/main/choreo/README.md
+bin/main/frc/robot/README.md

src/main/deploy/choreo/README.md

@@ -0,0 +1,23 @@
+## Autonomous Paths
+This folder is where all the paths that we made for autonomous pathing exist. Such paths were made using [Choreo](https://choreo.autos/), a path making software. Choreo is how we make all of our autonomous paths, and this year, we opted to make seperate paths, and chain them together for our routines.
+
+### How to make your own paths!
+If you would like to see what the paths look like for yourself, or even make your own, lucky for you its something thats very simple! First things first, you need to make sure that Choreo is installed([link for convienience](https://github.com/SleipnirGroup/Choreo/releases)), then navigate the menu (**LOCATED IN THE TOP LEFT**) to the *open project* tab. From there, navigate to where this cloned repository exists on *your* computer, and navigate to this folder. Select the ```Rebuilt.chor``` file and you're good to go! You can see the other paths there, and make your own from there as well.
+
+
+![Choreo Logo](images/choreoLogo.png)
+
+### Example of one of our autons
+```java
+        addMultiAuton(kLeftTrenchTwoCycleBumpReturn,
+            new AdaptableAutonInfo(AutonK.kLeftOneBumpReturn, AutonK.kSOTMTimeout, true, 0),
+            new AdaptableAutonInfo(AutonK.kLeftTwoBumpToTrench, AutonK.kSOTMTimeout, true, 0),
+            new AdaptableAutonInfo(AutonK.kLeftTwoBumpReturn, AutonK.kSOTMTimeout, true, 0),
+            new AdaptableAutonInfo(AutonK.kLeftTwoBumpToTrench, AutonK.kSOTMTimeout, true, 0),
+            new AdaptableAutonInfo(AutonK.kLeftTwoGoOut, AutonK.kSOTMTimeout, true, 0));
+```
+Note that we chain 5 paths together to create an auton, in which we run the RIGHT_one_bumpReturn, the RIGHT_two_bumpToTrench, the RIGHT_two_bumpReturn, followed by the RIGHT_two_bumpToTrench, finishing with the RIGHT_two_goOut. All of these paths exist as traj's in this folder, and that is where the code pulls from; such paths are, once again, made in choreo. So in order to make a new auton, you must make a plethora of new paths, and make it so that they chain together using the logic seen above.
+
+### Example of one of our paths (LEFT_one_bumpReturn)
+We start at the GREEN waypoint (waypoint one for those of you who are colorblind), and end at the RED waypoint (waypoint 7). The arrow on the square represents the front of our robot, and this year, that is our intake (counter-intuitive, tell me about it :sob:).
+![Choreo Path](images/choreoPath.png)

src/main/java/choreo/README.md

@@ -0,0 +1,47 @@
+# Choreo
+
+This folder contains our wrapper and utility classes for integrating [Choreo](https://choreo.autos/) into our robot code. Choreo is the path-planning software we use to generate and follow autonomous trajectories.
+
+## Why we forked/duplicated Choreo's library
+
+We duplicated Choreo's library classes locally so we could experiment with **pose correction** during path following. The idea was that if the robot drifts off its expected pose mid-path, we would pause the Choreo timer, drive the robot back to the correct pose, and then resume the timer once the robot is back on track. We weren't able to fully get this working in time, but the groundwork is here.
+```java
+    if(kUsePoseCorrection) {
+      if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) {
+          activeTimer.stop();
+          outOfBounds.set(true);
+      } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) {
+        activeTimer.start();
+        outOfBounds.set(false);
+      }
+    }
+```
+
+## Structure
+
+### auto/
+Handles autonomous routine selection and execution.
+
+- `AutoChooser.java`: lets drivers select an auto routine from the dashboard
+- `AutoFactory.java`: constructs auto routines by chaining trajectory segments together
+- `AutoRoutine.java`: represents a full autonomous routine made up of multiple trajectories
+- `AutoTrajectory.java`: wraps a single Choreo trajectory for command-based execution
+
+### trajectory/
+Core trajectory representation and sampling logic.
+
+- `Trajectory.java`: the main trajectory class, holds a list of samples and handles time-based lookups
+- `TrajectorySample.java`: a single sampled state along a trajectory (pose, velocity, etc.)
+- `SwerveSample.java`: trajectory sample specific to swerve drivetrains
+- `DifferentialSample.java`: trajectory sample specific to differential drivetrains
+- `EventMarker.java`: represents a timed event trigger within a trajectory
+
+### util/
+Utility and configuration classes.
+
+- `Choreo.java`: main entry point for loading trajectories from deploy files
+- `ChoreoAlert.java`: handles driver station alerts related to Choreo
+- `ChoreoAllianceFlipUtil.java`: flips trajectories for red/blue alliance mirroring
+- `ChoreoArrayUtil.java`: array helper methods used internally
+- `FieldDimensions.java`: field size constants used for alliance flipping
+- `TrajSchemaVersion.java`: version tracking for the trajectory file format

src/main/java/choreo/auto/AutoTrajectory.java

@@ -10,6 +10,7 @@
 import static choreo.util.ChoreoAlert.allianceNotReady;
 import static edu.wpi.first.wpilibj.Alert.AlertType.kError;
 import static edu.wpi.first.wpilibj.Alert.AlertType.kWarning;
+import static frc.robot.Constants.*;
 
 import choreo.Choreo.TrajectoryLogger;
 import choreo.auto.AutoFactory.AllianceContext;
@@ -178,13 +179,19 @@ private void cmdExecute() {
       return;
     }
     var sample = sampleOpt.get();
-    // if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) {
-    //     activeTimer.stop();
-    //     outOfBounds.set(true);
-    // } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) {
-    //   activeTimer.start();
-    //   outOfBounds.set(false);
-    // }
+    /*
+     * The entire idea of this block is to see that if we are outside of a certain range (0.2 meters in this case), we would pause the autonomous timer - * the timer just tells the robot where it should be, its nothing to 
+     * do with pose based, as choreo is time based * - until the robot reaches the pose (0.05 meters within tolerance in this case).
+     */
+    if(kUsePoseCorrection) {
+      if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) {
+          activeTimer.stop();
+          outOfBounds.set(true);
+      } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) {
+        activeTimer.start();
+        outOfBounds.set(false);
+      }
+    }
     if (sample instanceof SwerveSample swerveSample) {
       var swerveController = (Consumer<SwerveSample>) this.controller;
       swerveController.accept(swerveSample);