From a9431c942134de27d5329bbf84dc57f2b36a0f56 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sat, 1 Feb 2025 20:20:55 -0600
Subject: [PATCH 01/20] gitignore whatever my IDE is autogenerating

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index 2c96eb1..2759af1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
 target/
 Cargo.lock
+.DS_Store
+.idea/workspace.xml
\ No newline at end of file

From dc1d0ee7afd0cdbcb697c9dd931e7b0a7baab950 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sat, 1 Feb 2025 20:22:30 -0600
Subject: [PATCH 02/20] indexer was, as the comment suggested, going to move
 backwards

---
 src/subsystems/indexer.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index 474e817..e7b3d7f 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -31,7 +31,7 @@ impl Indexer {
         let mut last_loop = Instant::now();
 
         while self.laser_can.get_measurement() > robotmap::indexer::DISTANCE {
-            self.motor.set(ControlMode::Percent, 1.0); //may be -1 need to test
+            self.motor.set(ControlMode::Percent, -0.3); 
 
             // Cap at 250 hz
             let elapsed = last_loop.elapsed().as_secs_f64();

From 03fe263d785df5cf403307cfa884201ea4cbd83d Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sun, 2 Feb 2025 15:02:51 -0600
Subject: [PATCH 03/20] gitignore more ide stuff

---
 .gitignore | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index 2759af1..cb86d5a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
 target/
 Cargo.lock
 .DS_Store
-.idea/workspace.xml
\ No newline at end of file
+.idea/
\ No newline at end of file

From a689b124645d268295924a10a00395526c224fdb Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sun, 2 Feb 2025 15:05:27 -0600
Subject: [PATCH 04/20] Move laser distance constant, give it value, add
 get_laser_distance

---
 src/constants.rs          | 4 +++-
 src/subsystems/indexer.rs | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 139ba34..a822cba 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -24,7 +24,6 @@ pub mod robotmap {
 
     pub mod indexer {
         pub const MOTOR: i32 = 12;
-        pub const DISTANCE: i32 = 0;
     }
 
     pub mod climber {
@@ -73,6 +72,9 @@ pub mod elevator {
     pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async
     pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async
 }
+pub mod indexer {
+    pub const LASER_TRIP_DISTANCE_MM: i32 = 100;
+}
 pub mod joystick_map {
     // Joystick IDs (set in driver station)
     pub const RIGHT_DRIVE: i32 = 0;
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index e7b3d7f..8ce807d 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -4,6 +4,7 @@ use frcrs::laser_can::LaserCan;
 use std::time::Duration;
 use std::time::Instant;
 use tokio::time::sleep;
+use crate::constants;
 
 pub struct Indexer {
     motor: Talon,
@@ -30,7 +31,7 @@ impl Indexer {
     pub async fn intake_coral(&self) {
         let mut last_loop = Instant::now();
 
-        while self.laser_can.get_measurement() > robotmap::indexer::DISTANCE {
+        while self.laser_can.get_measurement() > constants::indexer::LASER_TRIP_DISTANCE_MM {
             self.motor.set(ControlMode::Percent, -0.3); 
 
             // Cap at 250 hz
@@ -41,6 +42,7 @@ impl Indexer {
         }
         self.motor.stop();
     }
+    pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}
 
     pub fn stop(&self) {
         self.motor.stop();

From 4bcb7b83238669f255e74e4cabd47e2d0f8eed5a Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sun, 2 Feb 2025 17:47:04 -0600
Subject: [PATCH 05/20] async elevator moves & commented out lasercan stuff

---
 src/constants.rs             |  2 ++
 src/lib.rs                   | 37 ++++++++++++++++++++++++++++++++++++
 src/subsystems/drivetrain.rs |  4 ++--
 src/subsystems/elevator.rs   |  3 +++
 src/subsystems/indexer.rs    | 11 ++++++-----
 5 files changed, 50 insertions(+), 7 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index a822cba..c89ff32 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -24,6 +24,7 @@ pub mod robotmap {
 
     pub mod indexer {
         pub const MOTOR: i32 = 12;
+        pub const LASER_CAN: i32 = 13;
     }
 
     pub mod climber {
@@ -91,6 +92,7 @@ pub mod joystick_map {
 
     //Operator
     pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET: usize = 1;
+    pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET_ASYNC: usize = 2;
     pub const ELEVATOR_UP_MANUAL: usize = 3;
     pub const ELEVATOR_DOWN_MANUAL: usize = 4;
     pub const CLIMB_FULL: usize = 8;
diff --git a/src/lib.rs b/src/lib.rs
index c15e605..9e2b34a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -19,6 +19,7 @@ use std::ops::Deref;
 use std::rc::Rc;
 use std::sync::Arc;
 use tokio::task::spawn_local;
+use crate::constants::elevator;
 
 #[derive(Clone)]
 pub struct Controllers {
@@ -45,6 +46,7 @@ pub struct Ferris {
     teleop_state: Rc<RefCell<TeleopState>>,
 
     auto_handle: Option<tokio::task::AbortHandle>,
+    elevator_trapezoid_handle: Option<tokio::task::AbortHandle>,
 }
 
 impl Default for Ferris {
@@ -70,6 +72,7 @@ impl Ferris {
             teleop_state: Default::default(),
 
             auto_handle: None,
+            elevator_trapezoid_handle: None,
         }
     }
 
@@ -223,6 +226,7 @@ impl Robot for Ferris {
         }
 
         if let Ok(indexer) = self.indexer.try_borrow_mut() {
+            //println!("laser dist: {}", indexer.get_laser_dist());
             if self
                 .controllers
                 .left_drive
@@ -242,6 +246,21 @@ impl Robot for Ferris {
             }
         }
 
+        if self.controllers.operator.get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET_ASYNC) {
+            //println!("button pressed");
+            if self.elevator_trapezoid_handle.is_none() {
+                //println!("Took elevator_trapezoid_handle");
+                let f = self.clone();
+                //println!("Cloned ferris");
+                let handle = spawn_local(elevator_move_to_target_async(f)).abort_handle();
+                //println!("Spawned task");
+                self.elevator_trapezoid_handle = Some(handle);
+            }
+        } else if let Some(handle) = self.elevator_trapezoid_handle.take() {
+            handle.abort();
+            //println!("Aborted elevator_trapezoid");
+        }
+
         // TODO: make more ergonomic, maybe move away from frcrs task manager in favor for abort handle in ferris struct
         // Untested
         let climber = Arc::clone(&self.climber);
@@ -287,3 +306,21 @@ impl Robot for Ferris {
         // println!("Test periodic");
     }
 }
+pub async fn elevator_move_to_target_async(robot: Ferris) {
+    //println!("Called elevator_move_to_target_async");
+    if let Ok(mut elevator) = robot.elevator.try_borrow_mut() {
+        //println!("Borrowed elevator");
+        let target_position = match elevator.get_target() {
+            ElevatorPosition::Bottom => elevator::BOTTOM,
+            ElevatorPosition::L2 => elevator::L2,
+            ElevatorPosition::L3 => elevator::L3,
+            ElevatorPosition::L4 => elevator::L4,
+        };
+        //println!("Error: {}", (elevator.get_position() - target_position).abs());
+        //println!("{}", (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE);
+        while (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE {
+            elevator.run_to_target_trapezoid();;
+        }
+        //println!("End of elevator_move_to_target_async");
+    }
+}
\ No newline at end of file
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index a5e17a0..1298d24 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -224,10 +224,10 @@ impl Drivetrain {
     }
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) {
-        println!(
+        /*println!(
             "ODO XY: {}, {}",
             self.odometry.position.x, self.odometry.position.y
-        );
+        );*/
         let mut transform = Vector2::new(-str, fwd);
         transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;
 
diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs
index 6e7ec0b..acdfb70 100644
--- a/src/subsystems/elevator.rs
+++ b/src/subsystems/elevator.rs
@@ -3,6 +3,7 @@ use frcrs::ctre::{ControlMode, Talon};
 use std::fmt::Display;
 use std::time::Duration;
 use tokio::time::sleep;
+use crate::constants;
 
 pub struct Elevator {
     left: Talon,
@@ -50,6 +51,8 @@ impl Elevator {
     pub fn get_target(&self) -> ElevatorPosition {
         self.target
     }
+    /// in rotations, from left motor
+    pub fn get_position(&self) -> f64 {self.left.get_position()}
 
     /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position.
     /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner).
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index 8ce807d..68cd826 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -8,7 +8,7 @@ use crate::constants;
 
 pub struct Indexer {
     motor: Talon,
-    laser_can: LaserCan,
+    //laser_can: LaserCan,
 }
 
 impl Default for Indexer {
@@ -20,15 +20,16 @@ impl Default for Indexer {
 impl Indexer {
     pub fn new() -> Self {
         let motor = Talon::new(robotmap::indexer::MOTOR, None);
+        //let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN);
 
-        Self { motor, laser_can }
+        Self { motor, /*laser_can*/ }
     }
 
     pub fn set_speed(&self, speed: f64) {
         self.motor.set(ControlMode::Percent, speed);
     }
 
-    pub async fn intake_coral(&self) {
+    /*pub async fn intake_coral(&self) {
         let mut last_loop = Instant::now();
 
         while self.laser_can.get_measurement() > constants::indexer::LASER_TRIP_DISTANCE_MM {
@@ -41,8 +42,8 @@ impl Indexer {
             last_loop = Instant::now();
         }
         self.motor.stop();
-    }
-    pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}
+    }*/
+    /*pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}*/
 
     pub fn stop(&self) {
         self.motor.stop();

From 4bc9a660e47eb4e703f0c6460bad7bc15f0b7216 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sun, 2 Feb 2025 20:38:03 -0600
Subject: [PATCH 06/20] docs for today's work

---
 src/constants.rs |  4 ++--
 src/lib.rs       | 19 ++++++++++++-------
 2 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index c89ff32..453604f 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -70,8 +70,8 @@ pub mod elevator {
     pub const L2: f64 = 1.; // unit is rotations
     pub const L3: f64 = 15.75; // unit is rotations
     pub const L4: f64 = 39.7; // unit is rotations
-    pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async
-    pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async
+    pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // sleep.await this long in between updating the elevator trapezoidal when running its async function
+    pub const POSITION_TOLERANCE: f64 = 0.25; // unit is rotations. finish elevator async move when within this distance of target
 }
 pub mod indexer {
     pub const LASER_TRIP_DISTANCE_MM: i32 = 100;
diff --git a/src/lib.rs b/src/lib.rs
index 9e2b34a..56e1b61 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -247,18 +247,23 @@ impl Robot for Ferris {
         }
 
         if self.controllers.operator.get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET_ASYNC) {
-            //println!("button pressed");
+            // If no abort handle is stored, this function isn't already running
+            // (prevents duplicate tasks)
             if self.elevator_trapezoid_handle.is_none() {
-                //println!("Took elevator_trapezoid_handle");
+                // Clone Ferris
+                // Functions called this way need to:
+                // be defined in the free space (not any impl block of lib)
+                // and take a robot struct to do their work with
+                // This prevents issues with values passing out of scope & such
                 let f = self.clone();
-                //println!("Cloned ferris");
+                // Start a task from the async function's returned future and set handle to the abort handle for that task
                 let handle = spawn_local(elevator_move_to_target_async(f)).abort_handle();
-                //println!("Spawned task");
+                // Store the abort handle in the Ferris struct
                 self.elevator_trapezoid_handle = Some(handle);
             }
         } else if let Some(handle) = self.elevator_trapezoid_handle.take() {
+            // If the button is no longer held, use the stored abort handle to abort the task & regain full robot control
             handle.abort();
-            //println!("Aborted elevator_trapezoid");
         }
 
         // TODO: make more ergonomic, maybe move away from frcrs task manager in favor for abort handle in ferris struct
@@ -307,7 +312,7 @@ impl Robot for Ferris {
     }
 }
 pub async fn elevator_move_to_target_async(robot: Ferris) {
-    //println!("Called elevator_move_to_target_async");
+    println!("Called elevator_move_to_target_async");
     if let Ok(mut elevator) = robot.elevator.try_borrow_mut() {
         //println!("Borrowed elevator");
         let target_position = match elevator.get_target() {
@@ -321,6 +326,6 @@ pub async fn elevator_move_to_target_async(robot: Ferris) {
         while (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE {
             elevator.run_to_target_trapezoid();;
         }
-        //println!("End of elevator_move_to_target_async");
+        println!("End of elevator_move_to_target_async");
     }
 }
\ No newline at end of file

From a09fb36bfda66dc1019f09b830c3ebdb68749d6b Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Mon, 3 Feb 2025 18:45:37 -0600
Subject: [PATCH 07/20] created lineup_2d

---
 Cargo.toml                   |  4 +--
 src/constants.rs             |  4 ++-
 src/lib.rs                   | 14 ++++----
 src/subsystems/drivetrain.rs | 62 ++++++++++++++++++++++++++++++++----
 src/subsystems/vision.rs     |  7 +++-
 5 files changed, 73 insertions(+), 18 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index e48c87d..3ebdc16 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -4,8 +4,8 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
-frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
-#frcrs = { path = "../../frcrs" }
+#frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
+frcrs = { path = "../frcrs" }
 tokio = { version = "1.36.0", features = ["rt", "full"] }
 serde = { version = "1.0.217", features = ["derive"] }
 serde_json = "1.0.134"
diff --git a/src/constants.rs b/src/constants.rs
index 453604f..0223b76 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -63,7 +63,9 @@ pub mod drivetrain {
     pub const SWERVE_DRIVE_SUGGESTION_ERR: f64 = 0.35;
     pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable
 
-    pub const PODIUM_SHOT_ANGLE: f64 = 34.34; // degrees
+
+    pub const LINEUP_2D_TX_KP: f64 = 0.01;
+    pub const LINEUP_2D_TY_KP: f64 = -0.0025;
 }
 pub mod elevator {
     pub const BOTTOM: f64 = 0.0; // unit is rotations
diff --git a/src/lib.rs b/src/lib.rs
index 56e1b61..feb4c7b 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -6,15 +6,14 @@ pub mod swerve;
 
 use crate::auto::Auto;
 use crate::container::control_drivetrain;
-use crate::subsystems::{
-    Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide,
-};
+use crate::subsystems::{Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide, Vision};
 use frcrs::input::Joystick;
 use frcrs::networktables::NetworkTable;
 use frcrs::telemetry::Telemetry;
 use frcrs::{Robot, TaskManager};
 use std::cell::RefCell;
 use std::cmp::PartialEq;
+use std::net::{IpAddr, Ipv4Addr, SocketAddr};
 use std::ops::Deref;
 use std::rc::Rc;
 use std::sync::Arc;
@@ -154,9 +153,10 @@ impl Robot for Ferris {
         let TeleopState {
             ref mut drivetrain_state,
         } = *self.teleop_state.deref().borrow_mut();
+        
 
         if let Ok(mut drivetrain) = self.drivetrain.try_borrow_mut() {
-            //drivetrain.update_limelight().await;
+            drivetrain.update_limelight().await;
             drivetrain.post_odo().await;
 
             if self
@@ -164,13 +164,13 @@ impl Robot for Ferris {
                 .right_drive
                 .get(constants::joystick_map::LINEUP_LEFT)
             {
-                drivetrain.lineup(LineupSide::Left).await;
+                drivetrain.lineup_2d_lower();
             } else if self
                 .controllers
                 .right_drive
                 .get(constants::joystick_map::LINEUP_RIGHT)
             {
-                drivetrain.lineup(LineupSide::Right).await;
+                drivetrain.lineup_2d_upper();
             } else {
                 control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await;
             }
@@ -240,7 +240,7 @@ impl Robot for Ferris {
                 .get(constants::joystick_map::INDEXER_IN)
             {
                 // In, score out the left
-                indexer.set_speed(-0.5);
+                indexer.set_speed(-0.17);
             } else {
                 indexer.set_speed(0.0);
             }
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 1298d24..6e73771 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -22,6 +22,7 @@ use crate::subsystems::Vision;
 use uom::si::angle::{degree, radian, revolution};
 use uom::si::f64::{Angle, Length};
 use uom::si::length::{inch, meter};
+use crate::constants;
 
 #[derive(Default)]
 pub struct DrivetrainControlState {
@@ -60,7 +61,8 @@ pub struct Drivetrain {
 
     pub offset: Angle,
 
-    pub vision: Vision,
+    pub limelight_lower: Vision,
+    pub limelight_upper: Vision,
 }
 
 #[derive(Serialize, Deserialize)]
@@ -89,10 +91,14 @@ impl Drivetrain {
         let bl_turn = Talon::new(BL_TURN, Some("can0".to_owned()));
         let br_turn = Talon::new(BR_TURN, Some("can0".to_owned()));
 
-        let vision = Vision::new(SocketAddr::new(
+        let limelight_lower = Vision::new(SocketAddr::new(
             IpAddr::V4(Ipv4Addr::new(10, 25, 2, 11)),
             5807,
         ));
+        let limelight_upper = Vision::new(SocketAddr::new(
+            IpAddr::V4(Ipv4Addr::new(10, 25, 2, 12)),
+            5807,
+        ));
 
         let offset = if alliance_station().red() {
             Angle::new::<degree>(180.)
@@ -120,16 +126,20 @@ impl Drivetrain {
 
             offset,
 
-            vision,
+            limelight_lower: limelight_lower,
+            limelight_upper: limelight_upper,
         }
     }
 
     pub async fn update_limelight(&mut self) {
-        self.vision
+        self.limelight_lower
+            .update(self.get_offset().get::<degree>() + 180.)
+            .await;
+        self.limelight_upper
             .update(self.get_offset().get::<degree>() + 180.)
             .await;
 
-        let pose = self.vision.get_botpose();
+        let pose = self.limelight_lower.get_botpose();
 
         // Calculate offset from robot center to limelight
         let robot_center_to_limelight_unrotated: Vector2<Length> = Vector2::new(
@@ -157,6 +167,8 @@ impl Drivetrain {
         if pose.x.get::<meter>() != 0.0 {
             self.update_odo(pose + robot_to_limelight);
         }
+        //println!("lower ll tx: {}", self.limelight_lower.get_tx().get::<degree>());
+        //println!("upper ll tx: {}", self.limelight_upper.get_tx().get::<degree>());
     }
 
     pub async fn post_odo(&self) {
@@ -372,12 +384,12 @@ impl Drivetrain {
     // The target position will be to the side of the apriltag, half a robot length away from the edge
     // Will account for the robot's orientation with the hexagon lineup
     pub fn calculate_target_lineup_position(&mut self, side: LineupSide) -> Option<LineupTarget> {
-        let tag_id = self.vision.get_saved_id();
+        let tag_id = self.limelight_lower.get_saved_id();
         if tag_id == -1 {
             return None;
         }
 
-        let tag_position = self.vision.get_tag_position(tag_id)?;
+        let tag_position = self.limelight_lower.get_tag_position(tag_id)?;
         let tag_coords = tag_position.coordinate?;
         let tag_rotation = tag_position.quaternion?;
 
@@ -419,6 +431,42 @@ impl Drivetrain {
             angle: robot_angle,
         })
     }
+
+    /// Lineup using tx and ty from the lower limelight
+    pub fn lineup_2d_lower(&mut self) {
+        if self.limelight_lower.get_id() != -1 {
+            let tag_position = self.limelight_lower.get_tag_position(self.limelight_lower.get_id()).unwrap();
+            let tag_rotation = tag_position.quaternion.unwrap();
+            let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
+            println!("perpendicular_yaw: {}", perpendicular_yaw);
+            println!("current angle: {}", self.get_offset().get::<radian>());
+            let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
+            self.set_speeds(
+                self.limelight_lower.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP,
+                self.limelight_lower.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP,
+                error_yaw * SWERVE_TURN_KP
+            );
+        } else {println!("can't lineup - no tag seen");}
+    }
+
+    /// Lineup using tx and ty from the upper limelight
+    pub fn lineup_2d_upper(&mut self) {
+        if self.limelight_upper.get_id() != -1 {
+            let tag_position = self.limelight_upper.get_tag_position(self.limelight_upper.get_id()).unwrap();
+            let tag_rotation = tag_position.quaternion.unwrap();
+            let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
+            println!("perpendicular_yaw: {}", perpendicular_yaw);
+            println!("current angle: {}", self.get_offset().get::<radian>());
+            let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
+            self.set_speeds(
+                self.limelight_upper.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP,
+                self.limelight_upper.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP,
+                error_yaw * SWERVE_TURN_KP
+            );
+        } else {println!("can't lineup - no tag seen");}
+    }
 }
 
 fn quaternion_to_yaw(quaternion: Quaternion<f64>) -> f64 {
diff --git a/src/subsystems/vision.rs b/src/subsystems/vision.rs
index 6545ed6..8b82b35 100644
--- a/src/subsystems/vision.rs
+++ b/src/subsystems/vision.rs
@@ -16,6 +16,7 @@ use uom::si::{
 
 use std::net::SocketAddr;
 
+#[derive(Clone)]
 pub struct Vision {
     tag_map_values: Value,
     limelight: Limelight,
@@ -78,7 +79,11 @@ impl Vision {
     /// Gets the id of the targeted tag as of the last update
     /// RETURNS -1 IF NO TAG FOUND
     pub fn get_id(&self) -> i32 {
-        self.results.Fiducial[0].fID
+        if self.results.Fiducial.len() != 0 {
+            self.results.Fiducial[0].fID
+        } else {
+            -1
+        }
     }
 
     pub fn get_saved_id(&self) -> i32 {

From f16897c44861e319bd09261f5485a42dd1bb73c2 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Tue, 4 Feb 2025 09:46:00 -0600
Subject: [PATCH 08/20] Refactor, document lineup_2d

---
 src/constants.rs             |  5 +++
 src/lib.rs                   |  4 +--
 src/subsystems/drivetrain.rs | 59 +++++++++++++++++++++---------------
 3 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 0223b76..6b90b50 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -66,6 +66,11 @@ pub mod drivetrain {
 
     pub const LINEUP_2D_TX_KP: f64 = 0.01;
     pub const LINEUP_2D_TY_KP: f64 = -0.0025;
+    pub const LINEUP_2D_TX_KS: f64 = 0.0;
+    pub const LINEUP_2D_TY_KS: f64 = 0.0;
+    pub const LINEUP_2D_TX_KD: f64 = 0.0;
+    pub const LINEUP_2D_TY_KD: f64 = 0.0;
+
 }
 pub mod elevator {
     pub const BOTTOM: f64 = 0.0; // unit is rotations
diff --git a/src/lib.rs b/src/lib.rs
index feb4c7b..49434fb 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -164,13 +164,13 @@ impl Robot for Ferris {
                 .right_drive
                 .get(constants::joystick_map::LINEUP_LEFT)
             {
-                drivetrain.lineup_2d_lower();
+                drivetrain.lineup_2d(LineupSide::Left);
             } else if self
                 .controllers
                 .right_drive
                 .get(constants::joystick_map::LINEUP_RIGHT)
             {
-                drivetrain.lineup_2d_upper();
+                drivetrain.lineup_2d(LineupSide::Right);
             } else {
                 control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await;
             }
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 6e73771..679f518 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -432,37 +432,46 @@ impl Drivetrain {
         })
     }
 
-    /// Lineup using tx and ty from the lower limelight
-    pub fn lineup_2d_lower(&mut self) {
-        if self.limelight_lower.get_id() != -1 {
-            let tag_position = self.limelight_lower.get_tag_position(self.limelight_lower.get_id()).unwrap();
+    /// Set drivetrain speeds using tx and ty from the lower limelight.
+    /// Cameras are positioned on the robot such that the tag on the base of the reef is in the exact center of the limelight's fov when the robot is fully lined up.
+    /// Uses a basic PID: tx from the limelight (horizontal position of the tag on the screen) feeds into drivetrain strafe, while ty feeds into drivetrain forward.
+    pub fn lineup_2d(&mut self, side: LineupSide) {
+        // The lower limelight points at the tag when lined up on the right, the upper when lined up on the left
+        let mut limelight = self.limelight_lower.clone();
+        match side {
+            LineupSide::Left => { limelight = self.limelight_upper.clone();}
+            LineupSide::Right => {limelight = self.limelight_lower.clone();}
+        }
+        
+        // Only try if a tag is detected
+        if limelight.get_id() != -1 {
+            // Figure out target angle from the tagmap
+            let tag_position = limelight.get_tag_position(limelight.get_id()).unwrap();
             let tag_rotation = tag_position.quaternion.unwrap();
+            // None of us actually know how the quaternions provided by said map work, this is copied code
+            // The pi stuff is to wrap the angle to the [-180, 180] range
             let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            // Tag yaw is in plane with the tag, not normal to (sticking out from) it - rotate by 90deg to fix that
             let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
-            println!("perpendicular_yaw: {}", perpendicular_yaw);
-            println!("current angle: {}", self.get_offset().get::<radian>());
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
-            self.set_speeds(
-                self.limelight_lower.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP,
-                self.limelight_lower.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP,
-                error_yaw * SWERVE_TURN_KP
-            );
-        } else {println!("can't lineup - no tag seen");}
-    }
 
-    /// Lineup using tx and ty from the upper limelight
-    pub fn lineup_2d_upper(&mut self) {
-        if self.limelight_upper.get_id() != -1 {
-            let tag_position = self.limelight_upper.get_tag_position(self.limelight_upper.get_id()).unwrap();
-            let tag_rotation = tag_position.quaternion.unwrap();
-            let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
-            let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
-            println!("perpendicular_yaw: {}", perpendicular_yaw);
-            println!("current angle: {}", self.get_offset().get::<radian>());
-            let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
+            // Calculate PID stuff
+            // KP - proportional: multiply the difference between where you are (tx or ty) and where you want to be (0) by a tuned constant (KP)
+            // KS - static: add a value to overcome static friction. Make sure it's in the right direction by multiplying by the proportional term divided by its absolute value (leaving only if it's positive or negative)
+            let fwd =
+                limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP
+                + constants::drivetrain::LINEUP_2D_TY_KS * (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP / (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP).abs());
+            let str =
+                self.limelight_lower.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP
+                + constants::drivetrain::LINEUP_2D_TX_KS * (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP / (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP).abs());
+            let mut transform = Vector2::new(str, fwd);
+
+            // rotate by (-1 * our drivetrain angle) to undo the rotation in set_speeds that lets us drive in field-relative mode
+            // since we want to drive in robot-relative mode here
+            transform = Rotation2::new( - self.get_offset().get::<radian>()) * transform;
             self.set_speeds(
-                self.limelight_upper.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP,
-                self.limelight_upper.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP,
+                transform.y,
+                transform.x,
                 error_yaw * SWERVE_TURN_KP
             );
         } else {println!("can't lineup - no tag seen");}

From 0ce048a71f8f333b0fc56ed7232e50b17be55db7 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Tue, 4 Feb 2025 11:48:17 -0600
Subject: [PATCH 09/20] Wrote code for lineup_2d derivative component, but set
 kD to 0

---
 src/subsystems/drivetrain.rs | 15 +++++++++------
 src/subsystems/vision.rs     |  4 ++++
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 679f518..279fc0e 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -442,7 +442,7 @@ impl Drivetrain {
             LineupSide::Left => { limelight = self.limelight_upper.clone();}
             LineupSide::Right => {limelight = self.limelight_lower.clone();}
         }
-        
+
         // Only try if a tag is detected
         if limelight.get_id() != -1 {
             // Figure out target angle from the tagmap
@@ -456,14 +456,17 @@ impl Drivetrain {
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
 
             // Calculate PID stuff
-            // KP - proportional: multiply the difference between where you are (tx or ty) and where you want to be (0) by a tuned constant (KP)
+            // KP - proportional: multiply the error (difference between where you are (tx or ty) and where you want to be (0)) by a tuned constant (KP)
+            // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
             // KS - static: add a value to overcome static friction. Make sure it's in the right direction by multiplying by the proportional term divided by its absolute value (leaving only if it's positive or negative)
             let fwd =
-                limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP
-                + constants::drivetrain::LINEUP_2D_TY_KS * (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP / (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP).abs());
+                constants::drivetrain::LINEUP_2D_TY_KP * limelight.get_ty().get::<degree>()
+                + constants::drivetrain::LINEUP_2D_TY_KS * (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP / (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP).abs())
+                + constants::drivetrain::LINEUP_2D_TY_KD * (limelight.get_ty().get::<degree>() - limelight.get_last_results().ty);
             let str =
-                self.limelight_lower.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP
-                + constants::drivetrain::LINEUP_2D_TX_KS * (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP / (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP).abs());
+                constants::drivetrain::LINEUP_2D_TX_KP * self.limelight_lower.get_tx().get::<degree>()
+                + constants::drivetrain::LINEUP_2D_TX_KS * (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP / (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP).abs())
+                + constants::drivetrain::LINEUP_2D_TX_KD * (limelight.get_tx().get::<degree>() - limelight.get_last_results().tx);
             let mut transform = Vector2::new(str, fwd);
 
             // rotate by (-1 * our drivetrain angle) to undo the rotation in set_speeds that lets us drive in field-relative mode
diff --git a/src/subsystems/vision.rs b/src/subsystems/vision.rs
index 8b82b35..10edbc5 100644
--- a/src/subsystems/vision.rs
+++ b/src/subsystems/vision.rs
@@ -21,6 +21,7 @@ pub struct Vision {
     tag_map_values: Value,
     limelight: Limelight,
     results: LimelightResults,
+    last_results: LimelightResults,
     saved_id: i32,
 }
 
@@ -42,11 +43,13 @@ impl Vision {
             tag_map_values: tag_values,
             limelight,
             results: LimelightResults::default(),
+            last_results: LimelightResults::default(),
             saved_id: 0,
         }
     }
     /// Updates the results from the limelight, also posts telemetry data
     pub async fn update(&mut self, dt_angle: f64) {
+        self.last_results = self.results.clone();
         self.results = self.limelight.results().await.unwrap();
         self.limelight
             .update_robot_orientation(dt_angle)
@@ -86,6 +89,7 @@ impl Vision {
         }
     }
 
+    pub fn get_last_results(&self) -> LimelightResults {self.last_results.clone()}
     pub fn get_saved_id(&self) -> i32 {
         self.saved_id
     }

From 8c32121588b300df4f11629bbe0292372f2a3f10 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Tue, 4 Feb 2025 16:22:16 -0600
Subject: [PATCH 10/20] More refactor more docs

---
 src/subsystems/drivetrain.rs | 24 +++++++++++++++---------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 279fc0e..e03aa5c 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -449,24 +449,30 @@ impl Drivetrain {
             let tag_position = limelight.get_tag_position(limelight.get_id()).unwrap();
             let tag_rotation = tag_position.quaternion.unwrap();
             // None of us actually know how the quaternions provided by said map work, this is copied code
-            // The pi stuff is to wrap the angle to the [-180, 180] range
+            // Flip the tag normal to be out the back of the tag and wrap to the [0, 360] range
             let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
-            // Tag yaw is in plane with the tag, not normal to (sticking out from) it - rotate by 90deg to fix that
+            // We score out the left, so forward-to-the-tag isn't very helpful
             let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
+
+            // Calculate errors (difference between where you are (tx, ty, or drivetrain angle) and where you want to be (0 deg, 0 deg, or perpendicular_yaw))
+            // Center of the limelight screen is (0,0) so we don't have to subtract anything for ty and tx
+            let error_ty = limelight.get_ty().get::<degree>();
+            let error_tx = limelight.get_tx().get::<degree>();
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
+            println!("target yaw: {} current yaw: {}", perpendicular_yaw, self.get_offset().get::<radian>());
 
             // Calculate PID stuff
-            // KP - proportional: multiply the error (difference between where you are (tx or ty) and where you want to be (0)) by a tuned constant (KP)
+            // KP - proportional: multiply the error by a tuned constant (KP)
             // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
             // KS - static: add a value to overcome static friction. Make sure it's in the right direction by multiplying by the proportional term divided by its absolute value (leaving only if it's positive or negative)
             let fwd =
-                constants::drivetrain::LINEUP_2D_TY_KP * limelight.get_ty().get::<degree>()
-                + constants::drivetrain::LINEUP_2D_TY_KS * (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP / (limelight.get_ty().get::<degree>() * constants::drivetrain::LINEUP_2D_TY_KP).abs())
-                + constants::drivetrain::LINEUP_2D_TY_KD * (limelight.get_ty().get::<degree>() - limelight.get_last_results().ty);
+                constants::drivetrain::LINEUP_2D_TY_KP * error_ty
+                + constants::drivetrain::LINEUP_2D_TY_KS * (error_ty * constants::drivetrain::LINEUP_2D_TY_KP / (error_ty * constants::drivetrain::LINEUP_2D_TY_KP).abs())
+                + constants::drivetrain::LINEUP_2D_TY_KD * (error_ty) - limelight.get_last_results().ty;
             let str =
-                constants::drivetrain::LINEUP_2D_TX_KP * self.limelight_lower.get_tx().get::<degree>()
-                + constants::drivetrain::LINEUP_2D_TX_KS * (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP / (limelight.get_tx().get::<degree>() * constants::drivetrain::LINEUP_2D_TX_KP).abs())
-                + constants::drivetrain::LINEUP_2D_TX_KD * (limelight.get_tx().get::<degree>() - limelight.get_last_results().tx);
+                constants::drivetrain::LINEUP_2D_TX_KP * error_tx
+                + constants::drivetrain::LINEUP_2D_TX_KS * (error_tx * constants::drivetrain::LINEUP_2D_TX_KP / (error_tx * constants::drivetrain::LINEUP_2D_TX_KP).abs())
+                + constants::drivetrain::LINEUP_2D_TX_KD * (error_tx - limelight.get_last_results().tx);
             let mut transform = Vector2::new(str, fwd);
 
             // rotate by (-1 * our drivetrain angle) to undo the rotation in set_speeds that lets us drive in field-relative mode

From e084f95a299cbd17f7fd82abcf3278b2f85f2bfe Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Tue, 4 Feb 2025 19:02:00 -0600
Subject: [PATCH 11/20] Fixed, tuned refactored lineup_2d

---
 src/constants.rs             |  8 +++---
 src/container.rs             |  4 +--
 src/subsystems/drivetrain.rs | 49 ++++++++++++++++++++++--------------
 3 files changed, 36 insertions(+), 25 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 6b90b50..52a1332 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -64,10 +64,10 @@ pub mod drivetrain {
     pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable
 
 
-    pub const LINEUP_2D_TX_KP: f64 = 0.01;
-    pub const LINEUP_2D_TY_KP: f64 = -0.0025;
-    pub const LINEUP_2D_TX_KS: f64 = 0.0;
-    pub const LINEUP_2D_TY_KS: f64 = 0.0;
+    pub const LINEUP_2D_TX_KP: f64 = -0.006;
+    pub const LINEUP_2D_TY_KP: f64 = -0.006;
+    pub const LINEUP_2D_TX_KS: f64 = 0.00;
+    pub const LINEUP_2D_TY_KS: f64 = 0.00;
     pub const LINEUP_2D_TX_KD: f64 = 0.0;
     pub const LINEUP_2D_TY_KD: f64 = 0.0;
 
diff --git a/src/container.rs b/src/container.rs
index 7e86b09..afb757e 100644
--- a/src/container.rs
+++ b/src/container.rs
@@ -1,5 +1,5 @@
 use crate::constants::drivetrain::SWERVE_TURN_KP;
-use crate::subsystems::{Drivetrain, DrivetrainControlState};
+use crate::subsystems::{Drivetrain, DrivetrainControlState, SwerveControlStyle};
 use crate::Controllers;
 use frcrs::deadzone;
 use nalgebra::ComplexField;
@@ -62,7 +62,7 @@ pub async fn control_drivetrain(
         deadrz
     };
 
-    drivetrain.set_speeds(deadly, deadlx, rot);
+    drivetrain.set_speeds(deadly, deadlx, rot, SwerveControlStyle::FieldOriented);
 
     if left_drive.get(4) {
         drivetrain.reset_heading();
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index e03aa5c..a51e7b2 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -34,6 +34,11 @@ pub enum LineupSide {
     Left,
     Right,
 }
+#[derive(Clone, Copy)]
+pub enum SwerveControlStyle {
+    RobotOriented,
+    FieldOriented,
+}
 
 #[derive(Debug)]
 pub struct LineupTarget {
@@ -235,13 +240,16 @@ impl Drivetrain {
         ((angle % 360.0) + 360.0) % 360.0
     }
 
-    pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) {
+    pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) {
         /*println!(
             "ODO XY: {}, {}",
             self.odometry.position.x, self.odometry.position.y
         );*/
         let mut transform = Vector2::new(-str, fwd);
-        transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;
+        match style {
+            SwerveControlStyle::FieldOriented => {transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;;},
+            SwerveControlStyle::RobotOriented => {},
+        }
 
         let wheel_speeds = self.kinematics.calculate(transform, -rot);
 
@@ -364,7 +372,7 @@ impl Drivetrain {
                 speed.x *= -1.
             }
 
-            self.set_speeds(speed.x, -speed.y, error_angle);
+            self.set_speeds(speed.x, -speed.y, error_angle, SwerveControlStyle::FieldOriented);
 
             Telemetry::put_number("error_position_x", error_position.x).await;
             Telemetry::put_number("error_position_y", error_position.y).await;
@@ -456,32 +464,35 @@ impl Drivetrain {
 
             // Calculate errors (difference between where you are (tx, ty, or drivetrain angle) and where you want to be (0 deg, 0 deg, or perpendicular_yaw))
             // Center of the limelight screen is (0,0) so we don't have to subtract anything for ty and tx
-            let error_ty = limelight.get_ty().get::<degree>();
-            let error_tx = limelight.get_tx().get::<degree>();
+            let error_ty = limelight.get_ty().get::<degree>() - 2.5;
+            let error_tx = limelight.get_tx().get::<degree>() - 8.;
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
-            println!("target yaw: {} current yaw: {}", perpendicular_yaw, self.get_offset().get::<radian>());
+            println!("hi3");
+
+            // Figure out correct direction for ks constants to apply in
+            let tx_dir = if constants::drivetrain::LINEUP_2D_TX_KP * error_tx > 0. {1.} else {-1.};
+            let ty_dir = if constants::drivetrain::LINEUP_2D_TY_KP * error_ty > 0. {1.} else {-1.};
 
             // Calculate PID stuff
             // KP - proportional: multiply the error by a tuned constant (KP)
             // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
-            // KS - static: add a value to overcome static friction. Make sure it's in the right direction by multiplying by the proportional term divided by its absolute value (leaving only if it's positive or negative)
-            let fwd =
-                constants::drivetrain::LINEUP_2D_TY_KP * error_ty
-                + constants::drivetrain::LINEUP_2D_TY_KS * (error_ty * constants::drivetrain::LINEUP_2D_TY_KP / (error_ty * constants::drivetrain::LINEUP_2D_TY_KP).abs())
-                + constants::drivetrain::LINEUP_2D_TY_KD * (error_ty) - limelight.get_last_results().ty;
+            // KS - static: add a value to overcome static friction.
             let str =
+                constants::drivetrain::LINEUP_2D_TY_KP * error_ty
+                + constants::drivetrain::LINEUP_2D_TY_KS * ty_dir
+                + constants::drivetrain::LINEUP_2D_TY_KD * (error_ty - limelight.get_last_results().ty);
+            let fwd =
                 constants::drivetrain::LINEUP_2D_TX_KP * error_tx
-                + constants::drivetrain::LINEUP_2D_TX_KS * (error_tx * constants::drivetrain::LINEUP_2D_TX_KP / (error_tx * constants::drivetrain::LINEUP_2D_TX_KP).abs())
-                + constants::drivetrain::LINEUP_2D_TX_KD * (error_tx - limelight.get_last_results().tx);
-            let mut transform = Vector2::new(str, fwd);
+                + constants::drivetrain::LINEUP_2D_TX_KS * tx_dir
+                + constants::drivetrain::LINEUP_2D_TX_KD * (error_tx - limelight.get_last_results().tx)
+                - constants::drivetrain::LINEUP_2D_TY_KP * error_ty;
+            let mut transform = Vector2::new(fwd, str);
 
-            // rotate by (-1 * our drivetrain angle) to undo the rotation in set_speeds that lets us drive in field-relative mode
-            // since we want to drive in robot-relative mode here
-            transform = Rotation2::new( - self.get_offset().get::<radian>()) * transform;
             self.set_speeds(
-                transform.y,
                 transform.x,
-                error_yaw * SWERVE_TURN_KP
+                transform.y,
+                error_yaw * SWERVE_TURN_KP,
+                SwerveControlStyle::RobotOriented
             );
         } else {println!("can't lineup - no tag seen");}
     }

From 060ccc872097f246d9dad5beb5debefb0d496c06 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 7 Feb 2025 16:52:45 -0600
Subject: [PATCH 12/20] auto path following

---
 Cargo.toml       |  2 +-
 src/auto/path.rs | 75 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 76 insertions(+), 1 deletion(-)

diff --git a/Cargo.toml b/Cargo.toml
index 3ebdc16..9e07a8c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -5,7 +5,7 @@ edition = "2021"
 
 [dependencies]
 #frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
-frcrs = { path = "../frcrs" }
+frcrs = { path = "../../frcrs" }
 tokio = { version = "1.36.0", features = ["rt", "full"] }
 serde = { version = "1.0.217", features = ["derive"] }
 serde_json = "1.0.134"
diff --git a/src/auto/path.rs b/src/auto/path.rs
index 8b13789..b4a98bc 100644
--- a/src/auto/path.rs
+++ b/src/auto/path.rs
@@ -1 +1,76 @@
+use std::{time::Duration};
+
+use nalgebra::{Vector2};
+use tokio::time::{Instant, sleep};
+use uom::si::{angle::{radian}, f64::{Length, Time}, length::{foot, meter}, time::{millisecond, second}, velocity::meter_per_second};
+use wpi_trajectory::{Path};
+
+use crate::{constants::drivetrain::{SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KF, SWERVE_DRIVE_KFA, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_DRIVE_MAX_ERR, SWERVE_TURN_KP}, subsystems::Drivetrain};
+use crate::subsystems::SwerveControlStyle;
+
+// TODO: Test
+pub async fn follow_path(drivetrain: &mut Drivetrain, path: Path) {
+    follow_path_range(drivetrain, path, SWERVE_DRIVE_MAX_ERR).await
+}
+
+pub async fn follow_path_range(drivetrain: &mut Drivetrain, path: Path, max_err: f64) {
+    let start = Instant::now();
+
+    let mut last_error = Vector2::zeros();
+    let mut last_loop = Instant::now();
+    let mut i = Vector2::zeros();
+
+    loop {
+        let now = Instant::now();
+        let dt = now-last_loop;
+        last_loop = now;
+
+        let elapsed = Time::new::<second>(start.elapsed().as_secs_f64());
+
+        let mut setpoint = path.get(elapsed);
+
+        let position = Vector2::new(setpoint.x.get::<meter>(), setpoint.y.get::<meter>());
+        let angle = -setpoint.heading;
+
+        let mut error_position = position - drivetrain.odometry.position;
+        let mut error_angle = (angle - drivetrain.get_angle()).get::<radian>();
+
+        if error_position.abs().max() < SWERVE_DRIVE_IE {
+            i += error_position;
+        }
+
+        if elapsed > path.length() && error_position.abs().max() < max_err && error_angle.abs() < 0.075  {
+            break;
+        }
+
+        error_angle *= SWERVE_TURN_KP;
+        error_position *= -SWERVE_DRIVE_KP;
+
+        let mut speed = error_position;
+
+        let velocity = Vector2::new(setpoint.velocity_x, setpoint.velocity_y);
+        let velocity = velocity.map(|x| x.get::<meter_per_second>());
+
+        let velocity_next = Vector2::new(setpoint.velocity_x, setpoint.velocity_y).map(|x| x.get::<meter_per_second>());
+
+        let acceleration = (velocity_next - velocity) * 1000./20.;
+
+        speed += velocity * -SWERVE_DRIVE_KF;
+        speed += acceleration * -SWERVE_DRIVE_KFA;
+        speed += i * -SWERVE_DRIVE_KI * dt.as_secs_f64() * 9.;
+
+        let speed_s = speed;
+        speed += (speed - last_error) * -SWERVE_DRIVE_KD * dt.as_secs_f64() * 9.;
+        last_error =  speed_s;
+
+        drivetrain.set_speeds(speed.x, speed.y, error_angle, SwerveControlStyle::FieldOriented);
+
+        drivetrain.update_odo(Vector2::new(
+            Length::new::<meter>(drivetrain.odometry.position.x),
+            Length::new::<meter>(drivetrain.odometry.position.y)
+        ));
+
+        sleep(Duration::from_millis(20)).await;
+    }
+}
 

From 69620c6fb9d67ac39ec915b327c1bbee0ae4d0c8 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 7 Feb 2025 17:00:09 -0600
Subject: [PATCH 13/20] basic auto path

---
 auto/BlueTriangle.traj | 117 +++++++++++++++++++++++++++++++++++++++++
 auto/paths.chor        |  78 +++++++++++++++++++++++++++
 2 files changed, 195 insertions(+)
 create mode 100644 auto/BlueTriangle.traj
 create mode 100644 auto/paths.chor

diff --git a/auto/BlueTriangle.traj b/auto/BlueTriangle.traj
new file mode 100644
index 0000000..1658e9f
--- /dev/null
+++ b/auto/BlueTriangle.traj
@@ -0,0 +1,117 @@
+{
+ "name":"BlueTriangle",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":8.075126647949219, "y":2.0993127822875977, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.206175327301025, "y":2.837056875228882, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.265194892883301, "y":0.9189223051071168, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":8.075126647949219, "y":2.0993127822875977, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"8.075126647949219 m", "val":8.075126647949219}, "y":{"exp":"2.0993127822875977 m", "val":2.0993127822875977}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.206175327301025 m", "val":6.206175327301025}, "y":{"exp":"2.837056875228882 m", "val":2.837056875228882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.265194892883301 m", "val":6.265194892883301}, "y":{"exp":"0.9189223051071167 m", "val":0.9189223051071168}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"8.075126647949219 m", "val":8.075126647949219}, "y":{"exp":"2.0993127822875977 m", "val":2.0993127822875977}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.14967,2.19995,3.43458],
+  "samples":[
+    {"t":0.0, "x":8.07513, "y":2.09931, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.98044, "ay":4.19861, "alpha":0.0, "fx":[-135.74501,-135.74501,-135.74501,-135.74501], "fy":[71.41722,71.41722,71.41722,71.41722]},
+    {"t":0.04599, "x":8.06669, "y":2.10375, "heading":0.0, "vx":-0.367, "vy":0.19308, "omega":0.0, "ax":-7.97772, "ay":4.19718, "alpha":0.0, "fx":[-135.6988,-135.6988,-135.6988,-135.6988], "fy":[71.39291,71.39291,71.39291,71.39291]},
+    {"t":0.09197, "x":8.04138, "y":2.11707, "heading":0.0, "vx":-0.73387, "vy":0.3861, "omega":0.0, "ax":-7.97232, "ay":4.19434, "alpha":0.0, "fx":[-135.60691,-135.60691,-135.60691,-135.60691], "fy":[71.34456,71.34456,71.34456,71.34456]},
+    {"t":0.13796, "x":7.9992, "y":2.13926, "heading":0.0, "vx":-1.10049, "vy":0.57898, "omega":0.0, "ax":-7.95636, "ay":4.18594, "alpha":0.0, "fx":[-135.33536,-135.33536,-135.33536,-135.33536], "fy":[71.2017,71.2017,71.2017,71.2017]},
+    {"t":0.18395, "x":7.94018, "y":2.17031, "heading":0.0, "vx":-1.46637, "vy":0.77148, "omega":0.0, "ax":-6.59366, "ay":3.46901, "alpha":0.0, "fx":[-112.15626,-112.15626,-112.15626,-112.15626], "fy":[59.00687,59.00687,59.00687,59.00687]},
+    {"t":0.22993, "x":7.86577, "y":2.20946, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":-0.00018, "ay":0.00009, "alpha":0.0, "fx":[-0.00307,-0.00307,-0.00307,-0.00307], "fy":[0.00162,0.00162,0.00162,0.00162]},
+    {"t":0.27592, "x":7.78439, "y":2.25227, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.32191, "x":7.70301, "y":2.29509, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.36789, "x":7.62164, "y":2.3379, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.41388, "x":7.54026, "y":2.38071, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.45987, "x":7.45888, "y":2.42353, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.50586, "x":7.3775, "y":2.46634, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.55184, "x":7.29612, "y":2.50916, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.59783, "x":7.21474, "y":2.55197, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.64382, "x":7.13336, "y":2.59479, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.6898, "x":7.05199, "y":2.6376, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,-0.00002,-0.00002,-0.00002], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]},
+    {"t":0.73579, "x":6.97061, "y":2.68041, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":-0.00003, "ay":-0.00005, "alpha":0.0, "fx":[-0.00044,-0.00044,-0.00044,-0.00044], "fy":[-0.00083,-0.00083,-0.00083,-0.00083]},
+    {"t":0.78178, "x":6.88923, "y":2.72323, "heading":0.0, "vx":-1.76961, "vy":0.93101, "omega":0.0, "ax":-0.00069, "ay":-0.00132, "alpha":0.0, "fx":[-0.01179,-0.01179,-0.01179,-0.01179], "fy":[-0.02241,-0.02241,-0.02241,-0.02241]},
+    {"t":0.82776, "x":6.80785, "y":2.76604, "heading":0.0, "vx":-1.76964, "vy":0.93095, "omega":0.0, "ax":-0.01875, "ay":-0.03569, "alpha":0.0, "fx":[-0.319,-0.319,-0.319,-0.319], "fy":[-0.60707,-0.60707,-0.60707,-0.60707]},
+    {"t":0.87375, "x":6.72645, "y":2.80882, "heading":0.0, "vx":-1.7705, "vy":0.92931, "omega":0.0, "ax":-0.48853, "ay":-0.95947, "alpha":0.0, "fx":[-8.3097,-8.3097,-8.3097,-8.3097], "fy":[-16.32029,-16.32029,-16.32029,-16.32029]},
+    {"t":0.91974, "x":6.64451, "y":2.85054, "heading":0.0, "vx":-1.79297, "vy":0.88519, "omega":0.0, "ax":-2.79557, "ay":-7.2209, "alpha":0.0, "fx":[-47.55185,-47.55185,-47.55185,-47.55185], "fy":[-122.82542,-122.82542,-122.82542,-122.82542]},
+    {"t":0.96572, "x":6.5591, "y":2.88361, "heading":0.0, "vx":-1.92153, "vy":0.55312, "omega":0.0, "ax":-1.56969, "ay":-8.72645, "alpha":0.0, "fx":[-26.70005,-26.70005,-26.70005,-26.70005], "fy":[-148.43435,-148.43435,-148.43435,-148.43435]},
+    {"t":1.01171, "x":6.46908, "y":2.89982, "heading":0.0, "vx":-1.99371, "vy":0.15182, "omega":0.0, "ax":0.24856, "ay":-8.96434, "alpha":0.0, "fx":[4.22793,4.22793,4.22793,4.22793], "fy":[-152.48081,-152.48081,-152.48081,-152.48081]},
+    {"t":1.0577, "x":6.37766, "y":2.89732, "heading":0.0, "vx":-1.98228, "vy":-0.26042, "omega":0.0, "ax":2.10111, "ay":-8.74723, "alpha":0.0, "fx":[35.73935,35.73935,35.73935,35.73935], "fy":[-148.78781,-148.78781,-148.78781,-148.78781]},
+    {"t":1.10368, "x":6.28872, "y":2.87609, "heading":0.0, "vx":-1.88566, "vy":-0.66268, "omega":0.0, "ax":3.94358, "ay":-8.09856, "alpha":0.0, "fx":[67.07922,67.07922,67.07922,67.07922], "fy":[-137.75416,-137.75416,-137.75416,-137.75416]},
+    {"t":1.14967, "x":6.20618, "y":2.83706, "heading":0.0, "vx":-1.7043, "vy":-1.03511, "omega":0.0, "ax":5.33072, "ay":-7.26008, "alpha":0.0, "fx":[90.67403,90.67403,90.67403,90.67403], "fy":[-123.49192,-123.49192,-123.49192,-123.49192]},
+    {"t":1.19343, "x":6.1367, "y":2.78481, "heading":0.0, "vx":-1.47102, "vy":-1.35282, "omega":0.0, "ax":6.70106, "ay":-6.00193, "alpha":0.0, "fx":[113.98318,113.98318,113.98318,113.98318], "fy":[-102.09111,-102.09111,-102.09111,-102.09111]},
+    {"t":1.23719, "x":6.07874, "y":2.71986, "heading":0.0, "vx":-1.17777, "vy":-1.61548, "omega":0.0, "ax":7.73085, "ay":-4.55171, "alpha":0.0, "fx":[131.49957,131.49957,131.49957,131.49957], "fy":[-77.42326,-77.42326,-77.42326,-77.42326]},
+    {"t":1.28096, "x":6.0346, "y":2.6448, "heading":0.0, "vx":-0.83945, "vy":-1.81467, "omega":0.0, "ax":8.39872, "ay":-2.933, "alpha":0.0, "fx":[142.85983,142.85983,142.85983,142.85983], "fy":[-49.88952,-49.88952,-49.88952,-49.88952]},
+    {"t":1.32472, "x":6.00591, "y":2.56258, "heading":0.0, "vx":-0.47191, "vy":-1.94302, "omega":0.0, "ax":8.34838, "ay":-1.22651, "alpha":0.0, "fx":[142.00357,142.00357,142.00357,142.00357], "fy":[-20.86255,-20.86255,-20.86255,-20.86255]},
+    {"t":1.36848, "x":5.99325, "y":2.47638, "heading":0.0, "vx":-0.10657, "vy":-1.9967, "omega":0.0, "ax":2.83016, "ay":-0.06333, "alpha":0.0, "fx":[48.14017,48.14017,48.14017,48.14017], "fy":[-1.07715,-1.07715,-1.07715,-1.07715]},
+    {"t":1.41224, "x":5.99129, "y":2.38894, "heading":0.0, "vx":0.01728, "vy":-1.99947, "omega":0.0, "ax":0.13457, "ay":0.00136, "alpha":0.0, "fx":[2.28906,2.28906,2.28906,2.28906], "fy":[0.02314,0.02314,0.02314,0.02314]},
+    {"t":1.456, "x":5.99218, "y":2.30144, "heading":0.0, "vx":0.02317, "vy":-1.99941, "omega":0.0, "ax":0.00577, "ay":0.00007, "alpha":0.0, "fx":[0.09819,0.09819,0.09819,0.09819], "fy":[0.00114,0.00114,0.00114,0.00114]},
+    {"t":1.49977, "x":5.9932, "y":2.21394, "heading":0.0, "vx":0.02342, "vy":-1.99941, "omega":0.0, "ax":0.00025, "ay":0.0, "alpha":0.0, "fx":[0.00421,0.00421,0.00421,0.00421], "fy":[0.00005,0.00005,0.00005,0.00005]},
+    {"t":1.54353, "x":5.99422, "y":2.12644, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.00001, "ay":0.0, "alpha":0.0, "fx":[0.00018,0.00018,0.00018,0.00018], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.58729, "x":5.99525, "y":2.03894, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.63105, "x":5.99628, "y":1.95145, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.67481, "x":5.9973, "y":1.86395, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.71857, "x":5.99833, "y":1.77645, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.76234, "x":5.99935, "y":1.68895, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.00004, "ay":0.0, "alpha":0.0, "fx":[0.00071,0.00071,0.00071,0.00071], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":1.8061, "x":6.00038, "y":1.60146, "heading":0.0, "vx":0.02344, "vy":-1.99941, "omega":0.0, "ax":0.00098, "ay":0.00001, "alpha":0.0, "fx":[0.01661,0.01661,0.01661,0.01661], "fy":[0.00019,0.00019,0.00019,0.00019]},
+    {"t":1.84986, "x":6.0014, "y":1.51396, "heading":0.0, "vx":0.02348, "vy":-1.99941, "omega":0.0, "ax":0.02277, "ay":0.00027, "alpha":0.0, "fx":[0.38739,0.38739,0.38739,0.38739], "fy":[0.00465,0.00465,0.00465,0.00465]},
+    {"t":1.89362, "x":6.00245, "y":1.42646, "heading":0.0, "vx":0.02448, "vy":-1.99939, "omega":0.0, "ax":0.52918, "ay":0.00955, "alpha":0.0, "fx":[9.00127,9.00127,9.00127,9.00127], "fy":[0.16251,0.16251,0.16251,0.16251]},
+    {"t":1.93738, "x":6.00403, "y":1.33897, "heading":0.0, "vx":0.04763, "vy":-1.99898, "omega":0.0, "ax":6.28872, "ay":0.58681, "alpha":0.0, "fx":[106.96941,106.96941,106.96941,106.96941], "fy":[9.98147,9.98147,9.98147,9.98147]},
+    {"t":1.98115, "x":6.01214, "y":1.25206, "heading":0.0, "vx":0.32284, "vy":-1.9733, "omega":0.0, "ax":8.48329, "ay":2.24286, "alpha":0.0, "fx":[144.29828,144.29828,144.29828,144.29828], "fy":[38.15046,38.15046,38.15046,38.15046]},
+    {"t":2.02491, "x":6.03439, "y":1.16785, "heading":0.0, "vx":0.69408, "vy":-1.87514, "omega":0.0, "ax":8.04232, "ay":3.91331, "alpha":0.0, "fx":[136.79755,136.79755,136.79755,136.79755], "fy":[66.56434,66.56434,66.56434,66.56434]},
+    {"t":2.06867, "x":6.07246, "y":1.08954, "heading":0.0, "vx":1.04603, "vy":-1.70389, "omega":0.0, "ax":7.15375, "ay":5.43776, "alpha":0.0, "fx":[121.68321,121.68321,121.68321,121.68321], "fy":[92.49478,92.49478,92.49478,92.49478]},
+    {"t":2.11243, "x":6.12509, "y":1.02018, "heading":0.0, "vx":1.35909, "vy":-1.46592, "omega":0.0, "ax":5.92113, "ay":6.78081, "alpha":0.0, "fx":[100.71675,100.71675,100.71675,100.71675], "fy":[115.33956,115.33956,115.33956,115.33956]},
+    {"t":2.15619, "x":6.19024, "y":0.96252, "heading":0.0, "vx":1.61821, "vy":-1.16918, "omega":0.0, "ax":4.32624, "ay":7.9036, "alpha":0.0, "fx":[73.58817,73.58817,73.58817,73.58817], "fy":[134.43797,134.43797,134.43797,134.43797]},
+    {"t":2.19995, "x":6.26519, "y":0.91892, "heading":0.0, "vx":1.80754, "vy":-0.82331, "omega":0.0, "ax":3.09235, "ay":8.46349, "alpha":0.0, "fx":[52.60006,52.60006,52.60006,52.60006], "fy":[143.96162,143.96162,143.96162,143.96162]},
+    {"t":2.24568, "x":6.35108, "y":0.89012, "heading":0.0, "vx":1.94894, "vy":-0.4363, "omega":0.0, "ax":1.09366, "ay":8.93568, "alpha":0.0, "fx":[18.60291,18.60291,18.60291,18.60291], "fy":[151.99338,151.99338,151.99338,151.99338]},
+    {"t":2.29141, "x":6.44134, "y":0.87951, "heading":0.0, "vx":1.99895, "vy":-0.0277, "omega":0.0, "ax":-0.79311, "ay":8.94925, "alpha":0.0, "fx":[-13.49063,-13.49063,-13.49063,-13.49063], "fy":[152.22421,152.22421,152.22421,152.22421]},
+    {"t":2.33713, "x":6.53192, "y":0.8876, "heading":0.0, "vx":1.96268, "vy":0.38152, "omega":0.0, "ax":-2.58929, "ay":8.54977, "alpha":0.0, "fx":[-44.04309,-44.04309,-44.04309,-44.04309], "fy":[145.42922,145.42922,145.42922,145.42922]},
+    {"t":2.38286, "x":6.61896, "y":0.91399, "heading":0.0, "vx":1.84428, "vy":0.77247, "omega":0.0, "ax":-4.12822, "ay":7.6306, "alpha":0.0, "fx":[-70.21989,-70.21989,-70.21989,-70.21989], "fy":[129.79426,129.79426,129.79426,129.79426]},
+    {"t":2.42859, "x":6.69898, "y":0.95729, "heading":0.0, "vx":1.65551, "vy":1.12139, "omega":0.0, "ax":-2.50049, "ay":3.33719, "alpha":0.0, "fx":[-42.53258,-42.53258,-42.53258,-42.53258], "fy":[56.76466,56.76466,56.76466,56.76466]},
+    {"t":2.47431, "x":6.77206, "y":1.01206, "heading":0.0, "vx":1.54117, "vy":1.27399, "omega":0.0, "ax":-0.12696, "ay":0.15288, "alpha":0.0, "fx":[-2.15951,-2.15951,-2.15951,-2.15951], "fy":[2.6004,2.6004,2.6004,2.6004]},
+    {"t":2.52004, "x":6.8424, "y":1.07047, "heading":0.0, "vx":1.53537, "vy":1.28098, "omega":0.0, "ax":-0.00478, "ay":0.00573, "alpha":0.0, "fx":[-0.08131,-0.08131,-0.08131,-0.08131], "fy":[0.09744,0.09744,0.09744,0.09744]},
+    {"t":2.56577, "x":6.9126, "y":1.12905, "heading":0.0, "vx":1.53515, "vy":1.28125, "omega":0.0, "ax":-0.00018, "ay":0.00021, "alpha":0.0, "fx":[-0.00305,-0.00305,-0.00305,-0.00305], "fy":[0.00366,0.00366,0.00366,0.00366]},
+    {"t":2.61149, "x":6.9828, "y":1.18764, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00011,-0.00011,-0.00011,-0.00011], "fy":[0.00014,0.00014,0.00014,0.00014]},
+    {"t":2.65722, "x":7.053, "y":1.24623, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":2.70295, "x":7.1232, "y":1.30481, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.74867, "x":7.19339, "y":1.3634, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.7944, "x":7.26359, "y":1.42199, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.84013, "x":7.33379, "y":1.48058, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.88586, "x":7.40398, "y":1.53916, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.93158, "x":7.47418, "y":1.59775, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.97731, "x":7.54438, "y":1.65634, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":3.02304, "x":7.61457, "y":1.71493, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":3.06876, "x":7.68477, "y":1.77352, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":3.11449, "x":7.75497, "y":1.8321, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":3.16022, "x":7.82517, "y":1.89069, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":-0.00019, "ay":-0.00016, "alpha":0.0, "fx":[-0.00331,-0.00331,-0.00331,-0.00331], "fy":[-0.00276,-0.00276,-0.00276,-0.00276]},
+    {"t":3.20594, "x":7.89536, "y":1.94928, "heading":0.0, "vx":1.53513, "vy":1.28125, "omega":0.0, "ax":-5.91038, "ay":-4.93291, "alpha":0.0, "fx":[-100.53393,-100.53393,-100.53393,-100.53393], "fy":[-83.90742,-83.90742,-83.90742,-83.90742]},
+    {"t":3.25167, "x":7.95938, "y":2.00271, "heading":0.0, "vx":1.26487, "vy":1.05568, "omega":0.0, "ax":-6.90198, "ay":-5.76052, "alpha":0.0, "fx":[-117.40073,-117.40073,-117.40073,-117.40073], "fy":[-97.98475,-97.98475,-97.98475,-97.98475]},
+    {"t":3.2974, "x":8.01, "y":2.04496, "heading":0.0, "vx":0.94926, "vy":0.79227, "omega":0.0, "ax":-6.9159, "ay":-5.77213, "alpha":0.0, "fx":[-117.63747,-117.63747,-117.63747,-117.63747], "fy":[-98.18234,-98.18234,-98.18234,-98.18234]},
+    {"t":3.34312, "x":8.04618, "y":2.07515, "heading":0.0, "vx":0.63302, "vy":0.52833, "omega":0.0, "ax":-6.92062, "ay":-5.77608, "alpha":0.0, "fx":[-117.71784,-117.71784,-117.71784,-117.71784], "fy":[-98.24941,-98.24941,-98.24941,-98.24941]},
+    {"t":3.38885, "x":8.06789, "y":2.09327, "heading":0.0, "vx":0.31657, "vy":0.26421, "omega":0.0, "ax":-6.923, "ay":-5.77806, "alpha":0.0, "fx":[-117.75829,-117.75829,-117.75829,-117.75829], "fy":[-98.28318,-98.28318,-98.28318,-98.28318]},
+    {"t":3.43458, "x":8.07513, "y":2.09931, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/auto/paths.chor b/auto/paths.chor
new file mode 100644
index 0000000..cdfc74d
--- /dev/null
+++ b/auto/paths.chor
@@ -0,0 +1,78 @@
+{
+ "name":"paths",
+ "version":1,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11 in",
+    "val":0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11 in",
+    "val":-0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "mass":{
+   "exp":"150 lbs",
+   "val":68.0388555
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.5",
+   "val":6.5
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"1.2 N * m",
+   "val":1.2
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "side":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "back":{
+    "exp":"16 in",
+    "val":0.4064
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[]
+}

From f63c594b6af510451959a0a37328f0283049f85f Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 7 Feb 2025 17:34:14 -0600
Subject: [PATCH 14/20] use my trajectory-rs

---
 Cargo.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Cargo.toml b/Cargo.toml
index 9e07a8c..a99525c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -11,7 +11,7 @@ serde = { version = "1.0.217", features = ["derive"] }
 serde_json = "1.0.134"
 uom = { version = "0.35.0", features = ["f64"] }
 nalgebra = "0.32.3"
-wpi-trajectory = { git = "https://github.com/Speedy6451/trajectory-rs" }
+wpi-trajectory = { git = "https://github.com/Sha-dos/trajectory-rs" }
 axum = "0.7.4"
 # limelightlib-rust = { git = "https://github.com/LimelightVision/limelightlib-rust.git" }
 

From f7c0a9013d7a3df54abe8e1aec9e1ba0e7bb047e Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 7 Feb 2025 18:38:40 -0600
Subject: [PATCH 15/20] auto triangle path

---
 Makefile                     |  5 ++++
 src/auto/mod.rs              | 36 ++++++++++++++++++++--
 src/auto/path.rs             | 58 ++++++++++++++++++++++++++----------
 src/lib.rs                   |  8 +++--
 src/subsystems/drivetrain.rs |  5 ++--
 5 files changed, 90 insertions(+), 22 deletions(-)

diff --git a/Makefile b/Makefile
index 8bd98a1..7e5378f 100644
--- a/Makefile
+++ b/Makefile
@@ -2,6 +2,7 @@ LIB=RobotCode2025
 OUT=target/arm-unknown-linux-gnueabi/release/$(LIB)
 DEPLOY=javastub/src/main/deploy/$(LIB)
 TEAM=25.02
+PATHS=auto/*
 
 .PHONY: check
 check:
@@ -27,6 +28,10 @@ deploy-scp: $(OUT)
 	scp $(OUT) lvuser@10.$(TEAM).2:
 	ssh lvuser@10.$(TEAM).2 /usr/local/frc/bin/frcRunRobot.sh
 
+.PHONY: deploy-paths
+deploy-paths:
+	scp -r $(PATHS) admin@10.$(TEAM).2:/home/lvuser/deploy/choreo/
+
 # Deploys the "deploy" directory and robotcode
 .PHONY: deploy
 deploy: $(OUT)
diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 5d113a9..2bd62c7 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -1,18 +1,25 @@
 mod path;
 
+use std::ops::Deref;
+use std::time::Duration;
+use nalgebra::Vector2;
 use serde::{Deserialize, Serialize};
+use tokio::time::sleep;
+use crate::auto::path::drive;
 
 use crate::Ferris;
 
 #[derive(Serialize, Deserialize)]
 pub enum Auto {
     Nothing,
+    BlueTriangle,
 }
 
 impl Auto {
     pub fn from_dashboard(s: &str) -> Self {
         match s {
             "Nothing" => Auto::Nothing,
+            "BlueTriangle" => Auto::BlueTriangle,
             _ => Auto::Nothing,
         }
     }
@@ -20,12 +27,13 @@ impl Auto {
     pub fn name(&self) -> &'static str {
         match self {
             Auto::Nothing => "Nothing",
+            Auto::BlueTriangle => "BlueTriangle",
             _ => "none",
         }
     }
 
     pub fn iterator() -> Vec<Self> {
-        vec![Auto::Nothing]
+        vec![Auto::Nothing, Auto::BlueTriangle]
     }
 
     pub fn names() -> Vec<String> {
@@ -35,9 +43,33 @@ impl Auto {
             .collect()
     }
 
-    pub async fn run_auto<'a>(_ferris: Ferris, chosen: Auto) {
+    pub async fn run_auto<'a>(ferris: Ferris, chosen: Auto) {
         match chosen {
             Auto::Nothing => {}
+            Auto::BlueTriangle => { blue_triangle(ferris).await.expect("Failed running auto"); }
         }
     }
 }
+
+pub async fn blue_triangle(robot: Ferris) -> Result<(), Box<dyn std::error::Error>> {
+    println!("RUNNING");
+
+    let mut drivetrain = robot.drivetrain.deref().borrow_mut();
+
+    drivetrain.odometry.set_abs(Vector2::new(8.075126647949219, 2.0993127822875977));
+
+    drive("BlueTriangle", &mut drivetrain, 1).await?;
+    println!("BlueTriangle.1 done");
+
+    sleep(Duration::from_secs_f64(1.)).await;
+
+    drive("BlueTriangle", &mut drivetrain, 2).await?;
+    println!("BlueTriangle.2 done");
+
+    sleep(Duration::from_secs_f64(1.)).await;
+
+    drive("BlueTriangle", &mut drivetrain, 3).await?;
+    println!("BlueTriangle.3 done");
+
+    Ok(())
+}
diff --git a/src/auto/path.rs b/src/auto/path.rs
index b4a98bc..20a11eb 100644
--- a/src/auto/path.rs
+++ b/src/auto/path.rs
@@ -1,6 +1,8 @@
 use std::{time::Duration};
+use tokio::fs::File;
 
 use nalgebra::{Vector2};
+use tokio::io::AsyncReadExt;
 use tokio::time::{Instant, sleep};
 use uom::si::{angle::{radian}, f64::{Length, Time}, length::{foot, meter}, time::{millisecond, second}, velocity::meter_per_second};
 use wpi_trajectory::{Path};
@@ -9,28 +11,58 @@ use crate::{constants::drivetrain::{SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRI
 use crate::subsystems::SwerveControlStyle;
 
 // TODO: Test
-pub async fn follow_path(drivetrain: &mut Drivetrain, path: Path) {
-    follow_path_range(drivetrain, path, SWERVE_DRIVE_MAX_ERR).await
+pub async fn drive(name: &str, drivetrain: &mut Drivetrain, waypoint_index: usize) -> Result<(), Box<dyn std::error::Error>> {
+    let mut path_content = String::new();
+    File::open(format!("/home/lvuser/deploy/choreo/{}.traj", name))
+        .await?
+        .read_to_string(&mut path_content)
+        .await?;
+
+    let path = Path::from_trajectory(&path_content)?;
+    let waypoints = path.waypoints();
+
+    if waypoint_index >= waypoints.len() {
+        return Err("Waypoint index out of bounds".into());
+    }
+
+    // Get start and end times for the waypoint
+    let start_time = if waypoint_index == 0 {
+        0.0
+    } else {
+        waypoints[waypoint_index - 1]
+    };
+    let end_time = waypoints[waypoint_index];
+
+    // Follow path for this segment
+    follow_path_segment(drivetrain, path, 0., 3.4).await;
+    drivetrain.set_speeds(0., 0., 0., SwerveControlStyle::FieldOriented);
+    Ok(())
 }
 
-pub async fn follow_path_range(drivetrain: &mut Drivetrain, path: Path, max_err: f64) {
+pub async fn follow_path_segment(drivetrain: &mut Drivetrain, path: Path, start_time: f64, end_time: f64) {
     let start = Instant::now();
-
     let mut last_error = Vector2::zeros();
     let mut last_loop = Instant::now();
     let mut i = Vector2::zeros();
 
     loop {
         let now = Instant::now();
-        let dt = now-last_loop;
+        let dt = now - last_loop;
         last_loop = now;
 
-        let elapsed = Time::new::<second>(start.elapsed().as_secs_f64());
+        //println!("x: {}, y: {}", drivetrain.odometry.position.x, drivetrain.odometry.position.y);
 
-        let mut setpoint = path.get(elapsed);
+        let elapsed = start.elapsed().as_secs_f64() + start_time;
+
+        // Exit if we've reached the end time for this segment
+        if elapsed > end_time {
+            break;
+        }
+
+        let setpoint = path.get(Time::new::<second>(elapsed));
 
-        let position = Vector2::new(setpoint.x.get::<meter>(), setpoint.y.get::<meter>());
         let angle = -setpoint.heading;
+        let position = Vector2::new(setpoint.x.get::<meter>(), setpoint.y.get::<meter>());
 
         let mut error_position = position - drivetrain.odometry.position;
         let mut error_angle = (angle - drivetrain.get_angle()).get::<radian>();
@@ -39,7 +71,7 @@ pub async fn follow_path_range(drivetrain: &mut Drivetrain, path: Path, max_err:
             i += error_position;
         }
 
-        if elapsed > path.length() && error_position.abs().max() < max_err && error_angle.abs() < 0.075  {
+        if elapsed > path.length().get::<second>() && error_position.abs().max() < SWERVE_DRIVE_MAX_ERR && error_angle.abs() < 0.075  {
             break;
         }
 
@@ -65,12 +97,6 @@ pub async fn follow_path_range(drivetrain: &mut Drivetrain, path: Path, max_err:
 
         drivetrain.set_speeds(speed.x, speed.y, error_angle, SwerveControlStyle::FieldOriented);
 
-        drivetrain.update_odo(Vector2::new(
-            Length::new::<meter>(drivetrain.odometry.position.x),
-            Length::new::<meter>(drivetrain.odometry.position.y)
-        ));
-
         sleep(Duration::from_millis(20)).await;
     }
-}
-
+}
\ No newline at end of file
diff --git a/src/lib.rs b/src/lib.rs
index 49434fb..f6eb78a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -101,7 +101,7 @@ impl Robot for Ferris {
             serde_json::to_string(&Auto::names()).unwrap(),
         )
         .await;
-        Telemetry::put_string("selected auto", Auto::Nothing.name().to_string()).await;
+        Telemetry::put_string("selected auto", Auto::BlueTriangle.name().to_string()).await;
     }
 
     fn disabled_init(&mut self) {
@@ -144,7 +144,11 @@ impl Robot for Ferris {
                 let handle = spawn_local(auto_task).abort_handle();
                 self.auto_handle = Some(handle);
             } else {
-                eprintln!("Failed to get selected auto from telemetry.");
+                eprintln!("Failed to get selected auto from telemetry, running default");
+
+                let auto_task = Auto::run_auto(f, Auto::Nothing);
+                let handle = spawn_local(auto_task).abort_handle();
+                self.auto_handle = Some(handle);
             }
         }
     }
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index a51e7b2..1048532 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -241,10 +241,11 @@ impl Drivetrain {
     }
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) {
-        /*println!(
+        println!(
             "ODO XY: {}, {}",
             self.odometry.position.x, self.odometry.position.y
-        );*/
+        );
+
         let mut transform = Vector2::new(-str, fwd);
         match style {
             SwerveControlStyle::FieldOriented => {transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;;},

From b044b2f9d05d05b5510e27036976f75f81af2ec0 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Fri, 7 Feb 2025 19:58:10 -0600
Subject: [PATCH 16/20] Fixed lineup_2d for non-tag-18 tags

---
 src/constants.rs             |  4 ++--
 src/subsystems/drivetrain.rs | 23 +++++++++++++++++------
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 52a1332..b57def3 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -64,8 +64,8 @@ pub mod drivetrain {
     pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable
 
 
-    pub const LINEUP_2D_TX_KP: f64 = -0.006;
-    pub const LINEUP_2D_TY_KP: f64 = -0.006;
+    pub const LINEUP_2D_TX_KP: f64 = -0.0067;
+    pub const LINEUP_2D_TY_KP: f64 = -0.0067;
     pub const LINEUP_2D_TX_KS: f64 = 0.00;
     pub const LINEUP_2D_TY_KS: f64 = 0.00;
     pub const LINEUP_2D_TX_KD: f64 = 0.0;
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index a51e7b2..cde0146 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -241,10 +241,10 @@ impl Drivetrain {
     }
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) {
-        /*println!(
+        println!(
             "ODO XY: {}, {}",
             self.odometry.position.x, self.odometry.position.y
-        );*/
+        );
         let mut transform = Vector2::new(-str, fwd);
         match style {
             SwerveControlStyle::FieldOriented => {transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;;},
@@ -458,21 +458,32 @@ impl Drivetrain {
             let tag_rotation = tag_position.quaternion.unwrap();
             // None of us actually know how the quaternions provided by said map work, this is copied code
             // Flip the tag normal to be out the back of the tag and wrap to the [0, 360] range
-            let tag_yaw = (quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            let tag_yaw = -(quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            println!("tag_yaw flipped: {}", tag_yaw);
             // We score out the left, so forward-to-the-tag isn't very helpful
-            let perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
+            let mut perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
+            // Convert to angle on [0,180]
+            if perpendicular_yaw > std::f64::consts::PI {
+                perpendicular_yaw = perpendicular_yaw - (std::f64::consts::PI * 2.);
+            }
 
             // Calculate errors (difference between where you are (tx, ty, or drivetrain angle) and where you want to be (0 deg, 0 deg, or perpendicular_yaw))
             // Center of the limelight screen is (0,0) so we don't have to subtract anything for ty and tx
             let error_ty = limelight.get_ty().get::<degree>() - 2.5;
             let error_tx = limelight.get_tx().get::<degree>() - 8.;
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
-            println!("hi3");
+            println!("hi, yaw: {} target yaw: {}", self.get_offset().get::<radian>(), perpendicular_yaw);
 
             // Figure out correct direction for ks constants to apply in
             let tx_dir = if constants::drivetrain::LINEUP_2D_TX_KP * error_tx > 0. {1.} else {-1.};
             let ty_dir = if constants::drivetrain::LINEUP_2D_TY_KP * error_ty > 0. {1.} else {-1.};
 
+            // Neither limelight faces perfectly straight out.
+            let off_straight_multiplier = match side {
+                LineupSide::Left => 1.0,
+                LineupSide::Right => -1.0,
+            };
+
             // Calculate PID stuff
             // KP - proportional: multiply the error by a tuned constant (KP)
             // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
@@ -485,7 +496,7 @@ impl Drivetrain {
                 constants::drivetrain::LINEUP_2D_TX_KP * error_tx
                 + constants::drivetrain::LINEUP_2D_TX_KS * tx_dir
                 + constants::drivetrain::LINEUP_2D_TX_KD * (error_tx - limelight.get_last_results().tx)
-                - constants::drivetrain::LINEUP_2D_TY_KP * error_ty;
+                + off_straight_multiplier * constants::drivetrain::LINEUP_2D_TY_KP * error_ty;
             let mut transform = Vector2::new(fwd, str);
 
             self.set_speeds(

From ed0f6183ff0d080998830eab7850a4af82d05f55 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Sat, 8 Feb 2025 12:20:28 -0600
Subject: [PATCH 17/20] intake coral command

---
 src/constants.rs          |  2 +-
 src/lib.rs                | 18 ++++++++++++++----
 src/subsystems/indexer.rs | 39 +++++++++++++++++++++++----------------
 3 files changed, 38 insertions(+), 21 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index b57def3..0bc2901 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -81,7 +81,7 @@ pub mod elevator {
     pub const POSITION_TOLERANCE: f64 = 0.25; // unit is rotations. finish elevator async move when within this distance of target
 }
 pub mod indexer {
-    pub const LASER_TRIP_DISTANCE_MM: i32 = 100;
+    pub const LASER_TRIP_DISTANCE_MM: i32 = 2;
 }
 pub mod joystick_map {
     // Joystick IDs (set in driver station)
diff --git a/src/lib.rs b/src/lib.rs
index f6eb78a..fa365c2 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -17,7 +17,10 @@ use std::net::{IpAddr, Ipv4Addr, SocketAddr};
 use std::ops::Deref;
 use std::rc::Rc;
 use std::sync::Arc;
-use tokio::task::spawn_local;
+use std::time::{Duration, Instant};
+use frcrs::ctre::ControlMode;
+use tokio::task::{AbortHandle, spawn_local};
+use tokio::time::sleep;
 use crate::constants::elevator;
 
 #[derive(Clone)]
@@ -46,6 +49,7 @@ pub struct Ferris {
 
     auto_handle: Option<tokio::task::AbortHandle>,
     elevator_trapezoid_handle: Option<tokio::task::AbortHandle>,
+    indexer_intake_handle: Option<AbortHandle>,
 }
 
 impl Default for Ferris {
@@ -72,6 +76,7 @@ impl Ferris {
 
             auto_handle: None,
             elevator_trapezoid_handle: None,
+            indexer_intake_handle: None,
         }
     }
 
@@ -243,10 +248,15 @@ impl Robot for Ferris {
                 .left_drive
                 .get(constants::joystick_map::INDEXER_IN)
             {
-                // In, score out the left
-                indexer.set_speed(-0.17);
+                    if self.indexer_intake_handle.is_none() {
+                        let f = self.clone();
+                        let handle = spawn_local(Indexer::intake_coral(f.indexer)).abort_handle();
+                        self.elevator_trapezoid_handle = Some(handle);
+                }
+            } else if let Some(handle) = self.elevator_trapezoid_handle.take() {
+                handle.abort();
             } else {
-                indexer.set_speed(0.0);
+                indexer.stop();
             }
         }
 
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index 68cd826..36f9911 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -1,14 +1,16 @@
+use std::cell::RefCell;
+use std::rc::Rc;
 use crate::constants::robotmap;
 use frcrs::ctre::{ControlMode, Talon};
 use frcrs::laser_can::LaserCan;
 use std::time::Duration;
 use std::time::Instant;
 use tokio::time::sleep;
-use crate::constants;
+use crate::{constants, Ferris};
 
 pub struct Indexer {
     motor: Talon,
-    //laser_can: LaserCan,
+    laser_can: LaserCan,
 }
 
 impl Default for Indexer {
@@ -20,30 +22,35 @@ impl Default for Indexer {
 impl Indexer {
     pub fn new() -> Self {
         let motor = Talon::new(robotmap::indexer::MOTOR, None);
-        //let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN);
+        let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN);
 
-        Self { motor, /*laser_can*/ }
+        Self { motor, laser_can }
     }
 
     pub fn set_speed(&self, speed: f64) {
         self.motor.set(ControlMode::Percent, speed);
     }
 
-    /*pub async fn intake_coral(&self) {
-        let mut last_loop = Instant::now();
+    pub async fn intake_coral(indexer: Rc<RefCell<Indexer>>) {
+        if let Ok(indexer) = indexer.try_borrow_mut() {
+            let mut last_loop = Instant::now();
 
-        while self.laser_can.get_measurement() > constants::indexer::LASER_TRIP_DISTANCE_MM {
-            self.motor.set(ControlMode::Percent, -0.3); 
+            while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM || indexer.laser_can.get_measurement() == -1 {
+                println!("Dist: {}", indexer.get_laser_dist());
+                indexer.set_speed(-0.3);
 
-            // Cap at 250 hz
-            let elapsed = last_loop.elapsed().as_secs_f64();
-            let left = (1. / 250. - elapsed).max(0.);
-            sleep(Duration::from_secs_f64(left)).await;
-            last_loop = Instant::now();
+                // Cap at 250 hz
+                let elapsed = last_loop.elapsed().as_secs_f64();
+                let left = (1. / 250. - elapsed).max(0.);
+                sleep(Duration::from_secs_f64(left)).await;
+                last_loop = Instant::now();
+            }
+
+            indexer.motor.stop();
         }
-        self.motor.stop();
-    }*/
-    /*pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}*/
+    }
+
+    pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}
 
     pub fn stop(&self) {
         self.motor.stop();

From 84fb25df4742ad972a1a0fccab627f3063db1bb1 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Sat, 8 Feb 2025 17:48:42 -0600
Subject: [PATCH 18/20] erm an entire control system

---
 src/auto/mod.rs              |  14 +-
 src/auto/path.rs             |  58 ++++++--
 src/constants.rs             |  16 ++-
 src/container.rs             |  21 ++-
 src/lib.rs                   | 268 +++++++++++++++--------------------
 src/subsystems/climber.rs    |  15 +-
 src/subsystems/drivetrain.rs |  71 +++++++---
 src/subsystems/elevator.rs   |  18 ++-
 src/subsystems/indexer.rs    |  22 ++-
 src/subsystems/vision.rs     |   4 +-
 10 files changed, 279 insertions(+), 228 deletions(-)

diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 2bd62c7..2989128 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -1,11 +1,11 @@
 mod path;
 
-use std::ops::Deref;
-use std::time::Duration;
+use crate::auto::path::drive;
 use nalgebra::Vector2;
 use serde::{Deserialize, Serialize};
+use std::ops::Deref;
+use std::time::Duration;
 use tokio::time::sleep;
-use crate::auto::path::drive;
 
 use crate::Ferris;
 
@@ -46,7 +46,9 @@ impl Auto {
     pub async fn run_auto<'a>(ferris: Ferris, chosen: Auto) {
         match chosen {
             Auto::Nothing => {}
-            Auto::BlueTriangle => { blue_triangle(ferris).await.expect("Failed running auto"); }
+            Auto::BlueTriangle => {
+                blue_triangle(ferris).await.expect("Failed running auto");
+            }
         }
     }
 }
@@ -56,7 +58,9 @@ pub async fn blue_triangle(robot: Ferris) -> Result<(), Box<dyn std::error::Erro
 
     let mut drivetrain = robot.drivetrain.deref().borrow_mut();
 
-    drivetrain.odometry.set_abs(Vector2::new(8.075126647949219, 2.0993127822875977));
+    drivetrain
+        .odometry
+        .set_abs(Vector2::new(8.075126647949219, 2.0993127822875977));
 
     drive("BlueTriangle", &mut drivetrain, 1).await?;
     println!("BlueTriangle.1 done");
diff --git a/src/auto/path.rs b/src/auto/path.rs
index 20a11eb..3557829 100644
--- a/src/auto/path.rs
+++ b/src/auto/path.rs
@@ -1,17 +1,33 @@
-use std::{time::Duration};
+use std::time::Duration;
 use tokio::fs::File;
 
-use nalgebra::{Vector2};
+use nalgebra::Vector2;
 use tokio::io::AsyncReadExt;
-use tokio::time::{Instant, sleep};
-use uom::si::{angle::{radian}, f64::{Length, Time}, length::{foot, meter}, time::{millisecond, second}, velocity::meter_per_second};
-use wpi_trajectory::{Path};
+use tokio::time::{sleep, Instant};
+use uom::si::{
+    angle::radian,
+    f64::{Length, Time},
+    length::{foot, meter},
+    time::{millisecond, second},
+    velocity::meter_per_second,
+};
+use wpi_trajectory::Path;
 
-use crate::{constants::drivetrain::{SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KF, SWERVE_DRIVE_KFA, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_DRIVE_MAX_ERR, SWERVE_TURN_KP}, subsystems::Drivetrain};
 use crate::subsystems::SwerveControlStyle;
+use crate::{
+    constants::drivetrain::{
+        SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KF, SWERVE_DRIVE_KFA, SWERVE_DRIVE_KI,
+        SWERVE_DRIVE_KP, SWERVE_DRIVE_MAX_ERR, SWERVE_TURN_KP,
+    },
+    subsystems::Drivetrain,
+};
 
 // TODO: Test
-pub async fn drive(name: &str, drivetrain: &mut Drivetrain, waypoint_index: usize) -> Result<(), Box<dyn std::error::Error>> {
+pub async fn drive(
+    name: &str,
+    drivetrain: &mut Drivetrain,
+    waypoint_index: usize,
+) -> Result<(), Box<dyn std::error::Error>> {
     let mut path_content = String::new();
     File::open(format!("/home/lvuser/deploy/choreo/{}.traj", name))
         .await?
@@ -39,7 +55,12 @@ pub async fn drive(name: &str, drivetrain: &mut Drivetrain, waypoint_index: usiz
     Ok(())
 }
 
-pub async fn follow_path_segment(drivetrain: &mut Drivetrain, path: Path, start_time: f64, end_time: f64) {
+pub async fn follow_path_segment(
+    drivetrain: &mut Drivetrain,
+    path: Path,
+    start_time: f64,
+    end_time: f64,
+) {
     let start = Instant::now();
     let mut last_error = Vector2::zeros();
     let mut last_loop = Instant::now();
@@ -71,7 +92,10 @@ pub async fn follow_path_segment(drivetrain: &mut Drivetrain, path: Path, start_
             i += error_position;
         }
 
-        if elapsed > path.length().get::<second>() && error_position.abs().max() < SWERVE_DRIVE_MAX_ERR && error_angle.abs() < 0.075  {
+        if elapsed > path.length().get::<second>()
+            && error_position.abs().max() < SWERVE_DRIVE_MAX_ERR
+            && error_angle.abs() < 0.075
+        {
             break;
         }
 
@@ -83,9 +107,10 @@ pub async fn follow_path_segment(drivetrain: &mut Drivetrain, path: Path, start_
         let velocity = Vector2::new(setpoint.velocity_x, setpoint.velocity_y);
         let velocity = velocity.map(|x| x.get::<meter_per_second>());
 
-        let velocity_next = Vector2::new(setpoint.velocity_x, setpoint.velocity_y).map(|x| x.get::<meter_per_second>());
+        let velocity_next = Vector2::new(setpoint.velocity_x, setpoint.velocity_y)
+            .map(|x| x.get::<meter_per_second>());
 
-        let acceleration = (velocity_next - velocity) * 1000./20.;
+        let acceleration = (velocity_next - velocity) * 1000. / 20.;
 
         speed += velocity * -SWERVE_DRIVE_KF;
         speed += acceleration * -SWERVE_DRIVE_KFA;
@@ -93,10 +118,15 @@ pub async fn follow_path_segment(drivetrain: &mut Drivetrain, path: Path, start_
 
         let speed_s = speed;
         speed += (speed - last_error) * -SWERVE_DRIVE_KD * dt.as_secs_f64() * 9.;
-        last_error =  speed_s;
+        last_error = speed_s;
 
-        drivetrain.set_speeds(speed.x, speed.y, error_angle, SwerveControlStyle::FieldOriented);
+        drivetrain.set_speeds(
+            speed.x,
+            speed.y,
+            error_angle,
+            SwerveControlStyle::FieldOriented,
+        );
 
         sleep(Duration::from_millis(20)).await;
     }
-}
\ No newline at end of file
+}
diff --git a/src/constants.rs b/src/constants.rs
index 0bc2901..7252539 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -28,8 +28,8 @@ pub mod robotmap {
     }
 
     pub mod climber {
-        pub const RAISE: i32 = 0;
-        pub const GRAB: i32 = 1;
+        pub const RAISE: i32 = 1;
+        pub const GRAB: i32 = 2;
     }
 }
 
@@ -63,14 +63,12 @@ pub mod drivetrain {
     pub const SWERVE_DRIVE_SUGGESTION_ERR: f64 = 0.35;
     pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable
 
-
     pub const LINEUP_2D_TX_KP: f64 = -0.0067;
     pub const LINEUP_2D_TY_KP: f64 = -0.0067;
     pub const LINEUP_2D_TX_KS: f64 = 0.00;
     pub const LINEUP_2D_TY_KS: f64 = 0.00;
     pub const LINEUP_2D_TX_KD: f64 = 0.0;
     pub const LINEUP_2D_TY_KD: f64 = 0.0;
-
 }
 pub mod elevator {
     pub const BOTTOM: f64 = 0.0; // unit is rotations
@@ -92,10 +90,16 @@ pub mod joystick_map {
     //Right drive
     pub const LINEUP_LEFT: usize = 3;
     pub const LINEUP_RIGHT: usize = 4;
+    pub const INTAKE: usize = 1;
+    pub const RESET_HEADING: usize = 5;
+    pub const CLIMB: usize = 2;
 
     //Left drive
-    pub const INDEXER_IN: usize = 1;
-    pub const INDEXER_OUT: usize = 2;
+    pub const SLOW_MODE: usize = 1;
+
+    pub const SCORE_L2: usize = 2;
+    pub const SCORE_L3: usize = 3;
+    pub const SCORE_L4: usize = 4;
 
     //Operator
     pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET: usize = 1;
diff --git a/src/container.rs b/src/container.rs
index afb757e..59c7eea 100644
--- a/src/container.rs
+++ b/src/container.rs
@@ -1,4 +1,5 @@
 use crate::constants::drivetrain::SWERVE_TURN_KP;
+use crate::constants::joystick_map::*;
 use crate::subsystems::{Drivetrain, DrivetrainControlState, SwerveControlStyle};
 use crate::Controllers;
 use frcrs::deadzone;
@@ -34,8 +35,16 @@ pub async fn control_drivetrain(
     let saved_angle = &mut state.saved_angle;
 
     let joystick_range = 0.04..1.;
-    let power_translate = if left_drive.get(1) { 0.0..0.3 } else { 0.0..1. };
-    let power_rotate = if left_drive.get(1) { 0.0..0.2 } else { 0.0..1. };
+    let power_translate = if left_drive.get(SLOW_MODE) {
+        0.0..0.3
+    } else {
+        0.0..1.
+    };
+    let power_rotate = if left_drive.get(SLOW_MODE) {
+        0.0..0.2
+    } else {
+        0.0..1.
+    };
     let deadly = deadzone(left_drive.get_y(), &joystick_range, &power_translate);
     let deadlx = deadzone(left_drive.get_x(), &joystick_range, &power_translate);
     let deadrz = deadzone(right_drive.get_z(), &joystick_range, &power_rotate);
@@ -53,18 +62,20 @@ pub async fn control_drivetrain(
         } else {
             0.
         }
-    } else if left_drive.get(2) {
+    }
+    /*else if right_drive.get(HOLD_90) {
         let angle = (drivetrain.get_angle() - drivetrain.offset).get::<degree>();
         let goal = (angle / 90.).round() * 90.;
         let error = angle - goal;
         -error.to_radians() * SWERVE_TURN_KP
-    } else {
+    } */
+    else {
         deadrz
     };
 
     drivetrain.set_speeds(deadly, deadlx, rot, SwerveControlStyle::FieldOriented);
 
-    if left_drive.get(4) {
+    if right_drive.get(RESET_HEADING) {
         drivetrain.reset_heading();
     }
 }
diff --git a/src/lib.rs b/src/lib.rs
index fa365c2..366e01a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -5,8 +5,14 @@ pub mod subsystems;
 pub mod swerve;
 
 use crate::auto::Auto;
+use crate::constants::elevator;
 use crate::container::control_drivetrain;
-use crate::subsystems::{Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide, Vision};
+use crate::subsystems::{
+    Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide,
+    Vision,
+};
+use constants::joystick_map::*;
+use frcrs::ctre::ControlMode;
 use frcrs::input::Joystick;
 use frcrs::networktables::NetworkTable;
 use frcrs::telemetry::Telemetry;
@@ -18,10 +24,8 @@ use std::ops::Deref;
 use std::rc::Rc;
 use std::sync::Arc;
 use std::time::{Duration, Instant};
-use frcrs::ctre::ControlMode;
-use tokio::task::{AbortHandle, spawn_local};
+use tokio::task::{spawn_local, AbortHandle};
 use tokio::time::sleep;
-use crate::constants::elevator;
 
 #[derive(Clone)]
 pub struct Controllers {
@@ -43,13 +47,14 @@ pub struct Ferris {
     pub drivetrain: Rc<RefCell<Drivetrain>>,
     pub elevator: Rc<RefCell<Elevator>>,
     pub indexer: Rc<RefCell<Indexer>>,
-    pub climber: Arc<Rc<RefCell<Climber>>>,
+    pub climber: Rc<RefCell<Climber>>,
 
     teleop_state: Rc<RefCell<TeleopState>>,
 
     auto_handle: Option<tokio::task::AbortHandle>,
     elevator_trapezoid_handle: Option<tokio::task::AbortHandle>,
     indexer_intake_handle: Option<AbortHandle>,
+    climb_handle: Option<AbortHandle>,
 }
 
 impl Default for Ferris {
@@ -70,13 +75,14 @@ impl Ferris {
             drivetrain: Rc::new(RefCell::new(Drivetrain::new())),
             elevator: Rc::new(RefCell::new(Elevator::new())),
             indexer: Rc::new(RefCell::new(Indexer::new())),
-            climber: Arc::new(Rc::new(RefCell::new(Climber::new()))),
+            climber: Rc::new(RefCell::new(Climber::new())),
 
             teleop_state: Default::default(),
 
             auto_handle: None,
             elevator_trapezoid_handle: None,
             indexer_intake_handle: None,
+            climb_handle: None,
         }
     }
 
@@ -162,162 +168,82 @@ impl Robot for Ferris {
         let TeleopState {
             ref mut drivetrain_state,
         } = *self.teleop_state.deref().borrow_mut();
-        
 
         if let Ok(mut drivetrain) = self.drivetrain.try_borrow_mut() {
-            drivetrain.update_limelight().await;
-            drivetrain.post_odo().await;
-
-            if self
-                .controllers
-                .right_drive
-                .get(constants::joystick_map::LINEUP_LEFT)
-            {
-                drivetrain.lineup_2d(LineupSide::Left);
-            } else if self
-                .controllers
-                .right_drive
-                .get(constants::joystick_map::LINEUP_RIGHT)
-            {
-                drivetrain.lineup_2d(LineupSide::Right);
-            } else {
-                control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await;
-            }
-        }
-
-        if let Ok(mut elevator) = self.elevator.try_borrow_mut() {
-            // Setting the target position
-            if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::SET_TARGET_L2)
-            {
-                elevator.set_target(ElevatorPosition::L2);
-            } else if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::SET_TARGET_L3)
-            {
-                elevator.set_target(ElevatorPosition::L3);
-            } else if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::SET_TARGET_L4)
-            {
-                elevator.set_target(ElevatorPosition::L4);
-            }
-
-            // Setting the actual elevator operation
-            if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::ELEVATOR_UP_MANUAL)
-            {
-                // Manual up
-                elevator.set_speed(0.1);
-            } else if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::ELEVATOR_DOWN_MANUAL)
-            {
-                // Manual down
-                elevator.set_speed(-0.1);
-            } else if self
-                .controllers
-                .operator
-                .get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET)
-            {
-                // Trapezoidal to stored target
-                elevator.run_to_target_trapezoid();
-            } else {
-                elevator.set_speed(0.0);
-            }
-        }
+            if let Ok(mut elevator) = self.elevator.try_borrow_mut() {
+                if let Ok(mut indexer) = self.indexer.try_borrow_mut() {
+                    drivetrain.update_limelight().await;
+                    drivetrain.post_odo().await;
+
+                    let side = if self.controllers.right_drive.get(LINEUP_LEFT) {
+                        LineupSide::Left
+                    } else if self.controllers.right_drive.get(LINEUP_RIGHT) {
+                        LineupSide::Right
+                    } else {
+                        LineupSide::Left
+                    };
 
-        if let Ok(indexer) = self.indexer.try_borrow_mut() {
-            //println!("laser dist: {}", indexer.get_laser_dist());
-            if self
-                .controllers
-                .left_drive
-                .get(constants::joystick_map::INDEXER_OUT)
-            {
-                // Out the front
-                indexer.set_speed(0.3);
-            } else if self
-                .controllers
-                .left_drive
-                .get(constants::joystick_map::INDEXER_IN)
-            {
-                    if self.indexer_intake_handle.is_none() {
-                        let f = self.clone();
-                        let handle = spawn_local(Indexer::intake_coral(f.indexer)).abort_handle();
-                        self.elevator_trapezoid_handle = Some(handle);
+                    if self.controllers.left_drive.get(SCORE_L2) {
+                        score(
+                            &mut drivetrain,
+                            &mut elevator,
+                            &mut indexer,
+                            ElevatorPosition::L2,
+                            side,
+                        )
+                    } else if self.controllers.left_drive.get(SCORE_L3) {
+                        score(
+                            &mut drivetrain,
+                            &mut elevator,
+                            &mut indexer,
+                            ElevatorPosition::L3,
+                            side,
+                        )
+                    } else if self.controllers.left_drive.get(SCORE_L4) {
+                        score(
+                            &mut drivetrain,
+                            &mut elevator,
+                            &mut indexer,
+                            ElevatorPosition::L4,
+                            side,
+                        )
+                    } else if self.controllers.right_drive.get(INTAKE) {
+                        elevator.set_target(ElevatorPosition::L2);
+                        elevator.run_to_target_trapezoid();
+
+                        if indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM
+                            || indexer.get_laser_dist() == -1
+                        {
+                            indexer.set_speed(-0.25);
+                        } else {
+                            indexer.stop();
+                        }
+                    } else {
+                        control_drivetrain(
+                            &mut drivetrain,
+                            &mut self.controllers,
+                            drivetrain_state,
+                        )
+                        .await;
+
+                        elevator.stop();
+                        indexer.stop();
+                    }
                 }
-            } else if let Some(handle) = self.elevator_trapezoid_handle.take() {
-                handle.abort();
-            } else {
-                indexer.stop();
             }
         }
 
-        if self.controllers.operator.get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET_ASYNC) {
-            // If no abort handle is stored, this function isn't already running
-            // (prevents duplicate tasks)
-            if self.elevator_trapezoid_handle.is_none() {
-                // Clone Ferris
-                // Functions called this way need to:
-                // be defined in the free space (not any impl block of lib)
-                // and take a robot struct to do their work with
-                // This prevents issues with values passing out of scope & such
+        if self.controllers.right_drive.get(CLIMB) {
+            if self.climb_handle.is_none() {
                 let f = self.clone();
-                // Start a task from the async function's returned future and set handle to the abort handle for that task
-                let handle = spawn_local(elevator_move_to_target_async(f)).abort_handle();
-                // Store the abort handle in the Ferris struct
-                self.elevator_trapezoid_handle = Some(handle);
-            }
-        } else if let Some(handle) = self.elevator_trapezoid_handle.take() {
-            // If the button is no longer held, use the stored abort handle to abort the task & regain full robot control
-            handle.abort();
-        }
-
-        // TODO: make more ergonomic, maybe move away from frcrs task manager in favor for abort handle in ferris struct
-        // Untested
-        let climber = Arc::clone(&self.climber);
-        let climb = {
-            let climber = Arc::clone(&climber);
-            move || {
-                let climber = Arc::clone(&climber);
-                async move {
-                    if let Ok(climber) = climber.try_borrow_mut() {
-                        climber.climb().await;
-                    };
-                }
+                let climb_task = Climber::climb(f);
+                let handle = spawn_local(climb_task).abort_handle();
+                self.climb_handle = Some(handle);
             }
-        };
-
-        let climber = Arc::clone(&self.climber);
-        let cancel_climb = {
-            let climber = Arc::clone(&climber);
-            move || {
-                let climber = Arc::clone(&climber);
-                async move {
-                    if let Ok(climber) = climber.try_borrow_mut() {
-                        climber.set_raise(false);
-                        climber.set_grab(false);
-                    };
-                }
-            }
-        };
-
-        if self
-            .controllers
-            .operator
-            .get(constants::joystick_map::CLIMB_FULL)
-        {
-            self.task_manager.run_task(climb);
         } else {
-            self.task_manager.run_task(cancel_climb);
-            self.task_manager.abort_task(climb);
+            if let Some(handle) = self.climb_handle.take() {
+                handle.abort();
+            }
         }
     }
 
@@ -338,8 +264,42 @@ pub async fn elevator_move_to_target_async(robot: Ferris) {
         //println!("Error: {}", (elevator.get_position() - target_position).abs());
         //println!("{}", (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE);
         while (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE {
-            elevator.run_to_target_trapezoid();;
+            elevator.run_to_target_trapezoid();
         }
         println!("End of elevator_move_to_target_async");
     }
-}
\ No newline at end of file
+}
+
+pub fn score(
+    drivetrain: &mut Drivetrain,
+    elevator: &mut Elevator,
+    indexer: &mut Indexer,
+    elevator_position: ElevatorPosition,
+    lineup_side: LineupSide,
+) {
+    let drivetrain_at_position = drivetrain.lineup_2d(lineup_side);
+    elevator.set_target(elevator_position);
+    let elevator_at_target = elevator.run_to_target_trapezoid();
+
+    if elevator_at_target && drivetrain_at_position {
+        if indexer.get_laser_dist() < constants::indexer::LASER_TRIP_DISTANCE_MM {
+            indexer.set_speed(-0.25);
+        } else {
+            indexer.stop();
+
+            elevator.set_target(ElevatorPosition::Bottom);
+            elevator.run_to_target_trapezoid();
+        }
+    }
+}
+
+pub fn after_score(robot: &Ferris) {
+    if let Ok(mut elevator) = robot.elevator.try_borrow_mut() {
+        if let Ok(mut indexer) = robot.indexer.try_borrow_mut() {
+            elevator.set_target(ElevatorPosition::Bottom);
+            elevator.run_to_target_trapezoid();
+
+            indexer.stop();
+        }
+    }
+}
diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs
index 4317ed3..b1df965 100644
--- a/src/subsystems/climber.rs
+++ b/src/subsystems/climber.rs
@@ -1,4 +1,5 @@
 use crate::constants::robotmap::climber::*;
+use crate::Ferris;
 use frcrs::solenoid::{ModuleType, Solenoid};
 use std::time::Duration;
 use tokio::time::sleep;
@@ -38,12 +39,14 @@ impl Climber {
         self.grab.set(engaged);
     }
 
-    pub async fn climb(&self) {
-        self.set_raise(true);
-        sleep(Duration::from_secs(1)).await;
-        self.set_grab(true);
-        sleep(Duration::from_secs_f64(0.25)).await;
-        self.set_raise(false);
+    pub async fn climb(ferris: Ferris) {
+        if let Ok(climber) = ferris.climber.try_borrow_mut() {
+            climber.set_raise(true);
+            sleep(Duration::from_secs(1)).await;
+            climber.set_grab(true);
+            sleep(Duration::from_secs_f64(0.25)).await;
+            climber.set_raise(false);
+        }
     }
 
     pub async fn reverse_climb(&self) {
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index cde0146..1150e1a 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -17,12 +17,12 @@ use nalgebra::{Quaternion, Rotation2, Vector2};
 use serde::Deserialize;
 use serde::Serialize;
 
+use crate::constants;
 use crate::constants::vision::ROBOT_CENTER_TO_LIMELIGHT_INCHES;
 use crate::subsystems::Vision;
 use uom::si::angle::{degree, radian, revolution};
 use uom::si::f64::{Angle, Length};
 use uom::si::length::{inch, meter};
-use crate::constants;
 
 #[derive(Default)]
 pub struct DrivetrainControlState {
@@ -247,8 +247,10 @@ impl Drivetrain {
         );
         let mut transform = Vector2::new(-str, fwd);
         match style {
-            SwerveControlStyle::FieldOriented => {transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;;},
-            SwerveControlStyle::RobotOriented => {},
+            SwerveControlStyle::FieldOriented => {
+                transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;
+            }
+            SwerveControlStyle::RobotOriented => {}
         }
 
         let wheel_speeds = self.kinematics.calculate(transform, -rot);
@@ -372,7 +374,12 @@ impl Drivetrain {
                 speed.x *= -1.
             }
 
-            self.set_speeds(speed.x, -speed.y, error_angle, SwerveControlStyle::FieldOriented);
+            self.set_speeds(
+                speed.x,
+                -speed.y,
+                error_angle,
+                SwerveControlStyle::FieldOriented,
+            );
 
             Telemetry::put_number("error_position_x", error_position.x).await;
             Telemetry::put_number("error_position_y", error_position.y).await;
@@ -443,12 +450,16 @@ impl Drivetrain {
     /// Set drivetrain speeds using tx and ty from the lower limelight.
     /// Cameras are positioned on the robot such that the tag on the base of the reef is in the exact center of the limelight's fov when the robot is fully lined up.
     /// Uses a basic PID: tx from the limelight (horizontal position of the tag on the screen) feeds into drivetrain strafe, while ty feeds into drivetrain forward.
-    pub fn lineup_2d(&mut self, side: LineupSide) {
+    pub fn lineup_2d(&mut self, side: LineupSide) -> bool {
         // The lower limelight points at the tag when lined up on the right, the upper when lined up on the left
         let mut limelight = self.limelight_lower.clone();
         match side {
-            LineupSide::Left => { limelight = self.limelight_upper.clone();}
-            LineupSide::Right => {limelight = self.limelight_lower.clone();}
+            LineupSide::Left => {
+                limelight = self.limelight_upper.clone();
+            }
+            LineupSide::Right => {
+                limelight = self.limelight_lower.clone();
+            }
         }
 
         // Only try if a tag is detected
@@ -458,7 +469,8 @@ impl Drivetrain {
             let tag_rotation = tag_position.quaternion.unwrap();
             // None of us actually know how the quaternions provided by said map work, this is copied code
             // Flip the tag normal to be out the back of the tag and wrap to the [0, 360] range
-            let tag_yaw = -(quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) % (std::f64::consts::PI * 2.);
+            let tag_yaw = -(quaternion_to_yaw(tag_rotation) + std::f64::consts::PI)
+                % (std::f64::consts::PI * 2.);
             println!("tag_yaw flipped: {}", tag_yaw);
             // We score out the left, so forward-to-the-tag isn't very helpful
             let mut perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
@@ -472,11 +484,23 @@ impl Drivetrain {
             let error_ty = limelight.get_ty().get::<degree>() - 2.5;
             let error_tx = limelight.get_tx().get::<degree>() - 8.;
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
-            println!("hi, yaw: {} target yaw: {}", self.get_offset().get::<radian>(), perpendicular_yaw);
+            println!(
+                "hi, yaw: {} target yaw: {}",
+                self.get_offset().get::<radian>(),
+                perpendicular_yaw
+            );
 
             // Figure out correct direction for ks constants to apply in
-            let tx_dir = if constants::drivetrain::LINEUP_2D_TX_KP * error_tx > 0. {1.} else {-1.};
-            let ty_dir = if constants::drivetrain::LINEUP_2D_TY_KP * error_ty > 0. {1.} else {-1.};
+            let tx_dir = if constants::drivetrain::LINEUP_2D_TX_KP * error_tx > 0. {
+                1.
+            } else {
+                -1.
+            };
+            let ty_dir = if constants::drivetrain::LINEUP_2D_TY_KP * error_ty > 0. {
+                1.
+            } else {
+                -1.
+            };
 
             // Neither limelight faces perfectly straight out.
             let off_straight_multiplier = match side {
@@ -488,14 +512,14 @@ impl Drivetrain {
             // KP - proportional: multiply the error by a tuned constant (KP)
             // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
             // KS - static: add a value to overcome static friction.
-            let str =
-                constants::drivetrain::LINEUP_2D_TY_KP * error_ty
+            let str = constants::drivetrain::LINEUP_2D_TY_KP * error_ty
                 + constants::drivetrain::LINEUP_2D_TY_KS * ty_dir
-                + constants::drivetrain::LINEUP_2D_TY_KD * (error_ty - limelight.get_last_results().ty);
-            let fwd =
-                constants::drivetrain::LINEUP_2D_TX_KP * error_tx
+                + constants::drivetrain::LINEUP_2D_TY_KD
+                    * (error_ty - limelight.get_last_results().ty);
+            let fwd = constants::drivetrain::LINEUP_2D_TX_KP * error_tx
                 + constants::drivetrain::LINEUP_2D_TX_KS * tx_dir
-                + constants::drivetrain::LINEUP_2D_TX_KD * (error_tx - limelight.get_last_results().tx)
+                + constants::drivetrain::LINEUP_2D_TX_KD
+                    * (error_tx - limelight.get_last_results().tx)
                 + off_straight_multiplier * constants::drivetrain::LINEUP_2D_TY_KP * error_ty;
             let mut transform = Vector2::new(fwd, str);
 
@@ -503,9 +527,18 @@ impl Drivetrain {
                 transform.x,
                 transform.y,
                 error_yaw * SWERVE_TURN_KP,
-                SwerveControlStyle::RobotOriented
+                SwerveControlStyle::RobotOriented,
             );
-        } else {println!("can't lineup - no tag seen");}
+
+            return if error_ty < 2. && error_tx < 2. && error_yaw < 2. {
+                true
+            } else {
+                false
+            };
+        } else {
+            println!("can't lineup - no tag seen");
+            false
+        }
     }
 }
 
diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs
index acdfb70..01d9d0b 100644
--- a/src/subsystems/elevator.rs
+++ b/src/subsystems/elevator.rs
@@ -1,9 +1,9 @@
+use crate::constants;
 use crate::constants::{elevator, robotmap};
 use frcrs::ctre::{ControlMode, Talon};
 use std::fmt::Display;
 use std::time::Duration;
 use tokio::time::sleep;
-use crate::constants;
 
 pub struct Elevator {
     left: Talon,
@@ -30,7 +30,7 @@ impl Elevator {
         let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string()));
         let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config
 
-        //right.follow(&left, true);
+        right.follow(&left, true);
 
         Self {
             left,
@@ -52,12 +52,14 @@ impl Elevator {
         self.target
     }
     /// in rotations, from left motor
-    pub fn get_position(&self) -> f64 {self.left.get_position()}
+    pub fn get_position(&self) -> f64 {
+        self.left.get_position()
+    }
 
     /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position.
     /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner).
     /// Those configuration files have been saved to Documents on the driver station laptop.
-    pub fn run_to_target_trapezoid(&mut self) {
+    pub fn run_to_target_trapezoid(&mut self) -> bool {
         //load position to run to in rotations from constants.rs
         let target_position = match self.get_target() {
             ElevatorPosition::Bottom => elevator::BOTTOM,
@@ -67,9 +69,15 @@ impl Elevator {
         };
 
         // current implementation is to just set the control mode and call this function every frame
-        self.right.set(ControlMode::MotionMagic, target_position);
+        //self.right.set(ControlMode::MotionMagic, target_position);
         self.left.set(ControlMode::MotionMagic, target_position);
 
+        if (target_position - self.right.get_position()).abs() < 1. {
+            true
+        } else {
+            false
+        }
+
         // below is for when we eventually make this function async
         /*
         while (self.right.get_position() - target_position).abs() < elevator::POSITION_TOLERANCE {
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index 36f9911..00af9c3 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -1,12 +1,12 @@
-use std::cell::RefCell;
-use std::rc::Rc;
 use crate::constants::robotmap;
+use crate::{constants, Ferris};
 use frcrs::ctre::{ControlMode, Talon};
 use frcrs::laser_can::LaserCan;
+use std::cell::RefCell;
+use std::rc::Rc;
 use std::time::Duration;
 use std::time::Instant;
 use tokio::time::sleep;
-use crate::{constants, Ferris};
 
 pub struct Indexer {
     motor: Talon,
@@ -33,24 +33,20 @@ impl Indexer {
 
     pub async fn intake_coral(indexer: Rc<RefCell<Indexer>>) {
         if let Ok(indexer) = indexer.try_borrow_mut() {
-            let mut last_loop = Instant::now();
-
-            while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM || indexer.laser_can.get_measurement() == -1 {
+            while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM
+                || indexer.laser_can.get_measurement() == -1
+            {
                 println!("Dist: {}", indexer.get_laser_dist());
                 indexer.set_speed(-0.3);
-
-                // Cap at 250 hz
-                let elapsed = last_loop.elapsed().as_secs_f64();
-                let left = (1. / 250. - elapsed).max(0.);
-                sleep(Duration::from_secs_f64(left)).await;
-                last_loop = Instant::now();
             }
 
             indexer.motor.stop();
         }
     }
 
-    pub fn get_laser_dist(&self) -> i32 {self.laser_can.get_measurement()}
+    pub fn get_laser_dist(&self) -> i32 {
+        self.laser_can.get_measurement()
+    }
 
     pub fn stop(&self) {
         self.motor.stop();
diff --git a/src/subsystems/vision.rs b/src/subsystems/vision.rs
index 10edbc5..9deedbb 100644
--- a/src/subsystems/vision.rs
+++ b/src/subsystems/vision.rs
@@ -89,7 +89,9 @@ impl Vision {
         }
     }
 
-    pub fn get_last_results(&self) -> LimelightResults {self.last_results.clone()}
+    pub fn get_last_results(&self) -> LimelightResults {
+        self.last_results.clone()
+    }
     pub fn get_saved_id(&self) -> i32 {
         self.saved_id
     }

From 269752a1149c61d99eb6927d885b10ed36113515 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Sat, 8 Feb 2025 18:41:07 -0600
Subject: [PATCH 19/20] better control scheme, auto scorerp make deploy-scp

---
 src/constants.rs             |  7 +++++--
 src/lib.rs                   | 23 +++++++++++++++++++++++
 src/subsystems/climber.rs    |  7 ++++++-
 src/subsystems/drivetrain.rs |  8 +++++---
 src/subsystems/elevator.rs   |  8 +++++---
 5 files changed, 44 insertions(+), 9 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 7252539..6747a2f 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -24,12 +24,12 @@ pub mod robotmap {
 
     pub mod indexer {
         pub const MOTOR: i32 = 12;
-        pub const LASER_CAN: i32 = 13;
+        pub const LASER_CAN: i32 = 0; // Cant save can id
     }
 
     pub mod climber {
         pub const RAISE: i32 = 1;
-        pub const GRAB: i32 = 2;
+        pub const GRAB: i32 = 0;
     }
 }
 
@@ -93,6 +93,9 @@ pub mod joystick_map {
     pub const INTAKE: usize = 1;
     pub const RESET_HEADING: usize = 5;
     pub const CLIMB: usize = 2;
+    pub const CLIMB_FALL: usize = 6;
+    pub const CLIMBER_GRAB: usize = 8;
+    pub const CLIMBER_RAISE: usize = 9;
 
     //Left drive
     pub const SLOW_MODE: usize = 1;
diff --git a/src/lib.rs b/src/lib.rs
index 366e01a..b412c9f 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -214,6 +214,7 @@ impl Robot for Ferris {
                         if indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM
                             || indexer.get_laser_dist() == -1
                         {
+                            println!("Dist: {}", indexer.get_laser_dist());
                             indexer.set_speed(-0.25);
                         } else {
                             indexer.stop();
@@ -240,10 +241,32 @@ impl Robot for Ferris {
                 let handle = spawn_local(climb_task).abort_handle();
                 self.climb_handle = Some(handle);
             }
+        } else if self.controllers.right_drive.get(CLIMB_FALL) {
+            if let Some(handle) = self.climb_handle.take() {
+                handle.abort();
+            }
+
+            if let Ok(mut climber) = self.climber.try_borrow_mut() {
+                climber.fall()
+            }
         } else {
             if let Some(handle) = self.climb_handle.take() {
                 handle.abort();
             }
+
+            if let Ok(mut climber) = self.climber.try_borrow_mut() {
+                if self.controllers.right_drive.get(CLIMBER_RAISE) {
+                    climber.set_raise(true);
+                } else {
+                    climber.set_raise(false);
+                }
+
+                if self.controllers.right_drive.get(CLIMBER_GRAB) {
+                    climber.set_grab(true);
+                } else {
+                    climber.set_grab(false);
+                }
+            }
         }
     }
 
diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs
index b1df965..32f945d 100644
--- a/src/subsystems/climber.rs
+++ b/src/subsystems/climber.rs
@@ -36,7 +36,7 @@ impl Climber {
     }
 
     pub fn set_grab(&self, engaged: bool) {
-        self.grab.set(engaged);
+        self.grab.set(!engaged);
     }
 
     pub async fn climb(ferris: Ferris) {
@@ -49,6 +49,11 @@ impl Climber {
         }
     }
 
+    pub fn fall(&self) {
+        self.set_grab(false);
+        self.set_raise(false);
+    }
+
     pub async fn reverse_climb(&self) {
         self.set_raise(true);
         sleep(Duration::from_secs_f64(0.25)).await;
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 1150e1a..0dbaf30 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -241,10 +241,10 @@ impl Drivetrain {
     }
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) {
-        println!(
+        /*println!(
             "ODO XY: {}, {}",
             self.odometry.position.x, self.odometry.position.y
-        );
+        );*/
         let mut transform = Vector2::new(-str, fwd);
         match style {
             SwerveControlStyle::FieldOriented => {
@@ -530,9 +530,11 @@ impl Drivetrain {
                 SwerveControlStyle::RobotOriented,
             );
 
-            return if error_ty < 2. && error_tx < 2. && error_yaw < 2. {
+            return if error_ty.abs() < 2. && error_tx.abs() < 2. && error_yaw.abs() < 0.2 {
+                println!("Drivetrain at target");
                 true
             } else {
+                println!("Drivetrain not at target ty: {} tx: {} yaw: {}", error_ty.abs(), error_tx.abs(), error_yaw.abs());
                 false
             };
         } else {
diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs
index 01d9d0b..4108d35 100644
--- a/src/subsystems/elevator.rs
+++ b/src/subsystems/elevator.rs
@@ -30,7 +30,7 @@ impl Elevator {
         let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string()));
         let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config
 
-        right.follow(&left, true);
+        //right.follow(&left, true);
 
         Self {
             left,
@@ -69,12 +69,14 @@ impl Elevator {
         };
 
         // current implementation is to just set the control mode and call this function every frame
-        //self.right.set(ControlMode::MotionMagic, target_position);
+        self.right.set(ControlMode::MotionMagic, target_position);
         self.left.set(ControlMode::MotionMagic, target_position);
 
-        if (target_position - self.right.get_position()).abs() < 1. {
+        if (target_position - self.right.get_position()).abs() < 0.5 {
+            println!("Elevator at target");
             true
         } else {
+            println!("Elevator not at target {}", (target_position - self.right.get_position()).abs());
             false
         }
 

From 5a64c36278bf7958449e66d91842da10b5762776 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Wed, 12 Feb 2025 07:02:29 -0600
Subject: [PATCH 20/20] Kind of fixed elevator,  lineup

---
 Cargo.toml                   |  2 +-
 src/constants.rs             | 20 +++++++-----
 src/lib.rs                   | 12 ++++++--
 src/subsystems/drivetrain.rs | 59 ++++++++++++++----------------------
 src/subsystems/elevator.rs   |  6 ++--
 5 files changed, 50 insertions(+), 49 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index a99525c..58e6e0a 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -5,7 +5,7 @@ edition = "2021"
 
 [dependencies]
 #frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
-frcrs = { path = "../../frcrs" }
+frcrs = { path = "../frcrs" }
 tokio = { version = "1.36.0", features = ["rt", "full"] }
 serde = { version = "1.0.217", features = ["derive"] }
 serde_json = "1.0.134"
diff --git a/src/constants.rs b/src/constants.rs
index 6747a2f..ceb8d9d 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -63,18 +63,24 @@ pub mod drivetrain {
     pub const SWERVE_DRIVE_SUGGESTION_ERR: f64 = 0.35;
     pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable
 
-    pub const LINEUP_2D_TX_KP: f64 = -0.0067;
-    pub const LINEUP_2D_TY_KP: f64 = -0.0067;
-    pub const LINEUP_2D_TX_KS: f64 = 0.00;
-    pub const LINEUP_2D_TY_KS: f64 = 0.00;
-    pub const LINEUP_2D_TX_KD: f64 = 0.0;
-    pub const LINEUP_2D_TY_KD: f64 = 0.0;
+    pub const LINEUP_2D_TX_STR_KP: f64 = 0.005;
+    pub const LINEUP_2D_TX_FWD_KP: f64 = 0.005;
+    pub const LINEUP_2D_TY_STR_KP: f64 = 0.005;
+    pub const LINEUP_2D_TY_FWD_KP: f64 = 0.005;
+    pub const TARGET_TY_LEFT: f64 = -7.4;
+    pub const TARGET_TY_RIGHT: f64 = 3.45;
+    pub const TARGET_TX_LEFT: f64 = -7.4;
+    pub const TARGET_TX_RIGHT: f64 = 3.45;
+    pub const TX_ACCEPTABLE_ERROR: f64 = 1.8;
+    pub const TY_ACCEPTABLE_ERROR: f64 = 1.8;
+    pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02;
+
 }
 pub mod elevator {
     pub const BOTTOM: f64 = 0.0; // unit is rotations
     pub const L2: f64 = 1.; // unit is rotations
     pub const L3: f64 = 15.75; // unit is rotations
-    pub const L4: f64 = 39.7; // unit is rotations
+    pub const L4: f64 = 38.8; // unit is rotations
     pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // sleep.await this long in between updating the elevator trapezoidal when running its async function
     pub const POSITION_TOLERANCE: f64 = 0.25; // unit is rotations. finish elevator async move when within this distance of target
 }
diff --git a/src/lib.rs b/src/lib.rs
index b412c9f..4f5bc6d 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -306,12 +306,18 @@ pub fn score(
 
     if elevator_at_target && drivetrain_at_position {
         if indexer.get_laser_dist() < constants::indexer::LASER_TRIP_DISTANCE_MM {
-            indexer.set_speed(-0.25);
+            let indexer_speed = match elevator_position {
+                ElevatorPosition::Bottom => -0.25,
+                ElevatorPosition::L2 => -0.25,
+                ElevatorPosition::L3 => -0.25,
+                ElevatorPosition::L4 => -0.1
+            };
+            indexer.set_speed(indexer_speed);
         } else {
             indexer.stop();
 
-            elevator.set_target(ElevatorPosition::Bottom);
-            elevator.run_to_target_trapezoid();
+            //elevator.set_target(ElevatorPosition::Bottom);
+            //elevator.run_to_target_trapezoid();
         }
     }
 }
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 0dbaf30..f478e1f 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -5,9 +5,7 @@ use std::ops::Add;
 
 use frcrs::ctre::{talon_encoder_tick, ControlMode, Pigeon, Talon};
 
-use crate::constants::drivetrain::{
-    SWERVE_DRIVE_IE, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP,
-};
+use crate::constants::drivetrain::{LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, SWERVE_DRIVE_IE, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, TARGET_TX_LEFT, TARGET_TX_RIGHT, TARGET_TY_LEFT, TARGET_TY_RIGHT, TX_ACCEPTABLE_ERROR, TY_ACCEPTABLE_ERROR, YAW_ACCEPTABLE_ERROR};
 use crate::constants::robotmap::swerve::*;
 use crate::swerve::kinematics::{ModuleState, Swerve};
 use crate::swerve::odometry::{ModuleReturn, Odometry};
@@ -471,56 +469,45 @@ impl Drivetrain {
             // Flip the tag normal to be out the back of the tag and wrap to the [0, 360] range
             let tag_yaw = -(quaternion_to_yaw(tag_rotation) + std::f64::consts::PI)
                 % (std::f64::consts::PI * 2.);
-            println!("tag_yaw flipped: {}", tag_yaw);
             // We score out the left, so forward-to-the-tag isn't very helpful
             let mut perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0;
             // Convert to angle on [0,180]
             if perpendicular_yaw > std::f64::consts::PI {
                 perpendicular_yaw = perpendicular_yaw - (std::f64::consts::PI * 2.);
             }
+            let target_ty = match side {
+                LineupSide::Left => TARGET_TY_LEFT,
+                LineupSide::Right => TARGET_TY_RIGHT
+            };
+            let target_tx = match side {
+                LineupSide::Left => TARGET_TX_LEFT,
+                LineupSide::Right => TARGET_TX_RIGHT,
+            };
 
             // Calculate errors (difference between where you are (tx, ty, or drivetrain angle) and where you want to be (0 deg, 0 deg, or perpendicular_yaw))
             // Center of the limelight screen is (0,0) so we don't have to subtract anything for ty and tx
-            let error_ty = limelight.get_ty().get::<degree>() - 2.5;
-            let error_tx = limelight.get_tx().get::<degree>() - 8.;
+            let error_ty = target_ty - limelight.get_ty().get::<degree>();
+            let error_tx = target_tx - limelight.get_tx().get::<degree>();
             let error_yaw = perpendicular_yaw - self.get_offset().get::<radian>();
-            println!(
-                "hi, yaw: {} target yaw: {}",
-                self.get_offset().get::<radian>(),
-                perpendicular_yaw
-            );
-
-            // Figure out correct direction for ks constants to apply in
-            let tx_dir = if constants::drivetrain::LINEUP_2D_TX_KP * error_tx > 0. {
-                1.
-            } else {
-                -1.
-            };
-            let ty_dir = if constants::drivetrain::LINEUP_2D_TY_KP * error_ty > 0. {
-                1.
-            } else {
-                -1.
-            };
 
             // Neither limelight faces perfectly straight out.
-            let off_straight_multiplier = match side {
+            let off_straight_multiplier_str = match side {
+                LineupSide::Left => -1.0,
+                LineupSide::Right => 1.0,
+            };
+            let off_straight_multiplier_fwd = match side {
                 LineupSide::Left => 1.0,
                 LineupSide::Right => -1.0,
             };
 
             // Calculate PID stuff
             // KP - proportional: multiply the error by a tuned constant (KP)
-            // KD - derivative: subtract the error from last frame by the error from this frame (this difference is roughly proportional to the rate at which the error is changing), then multiply by a tuned constant (KD)
-            // KS - static: add a value to overcome static friction.
-            let str = constants::drivetrain::LINEUP_2D_TY_KP * error_ty
-                + constants::drivetrain::LINEUP_2D_TY_KS * ty_dir
-                + constants::drivetrain::LINEUP_2D_TY_KD
-                    * (error_ty - limelight.get_last_results().ty);
-            let fwd = constants::drivetrain::LINEUP_2D_TX_KP * error_tx
-                + constants::drivetrain::LINEUP_2D_TX_KS * tx_dir
-                + constants::drivetrain::LINEUP_2D_TX_KD
-                    * (error_tx - limelight.get_last_results().tx)
-                + off_straight_multiplier * constants::drivetrain::LINEUP_2D_TY_KP * error_ty;
+            let str = constants::drivetrain::LINEUP_2D_TY_STR_KP * error_ty
+                + off_straight_multiplier_str * LINEUP_2D_TX_STR_KP * error_tx;
+
+            let fwd = LINEUP_2D_TX_FWD_KP * error_tx
+                + off_straight_multiplier_fwd * LINEUP_2D_TY_FWD_KP * error_yaw;
+
             let mut transform = Vector2::new(fwd, str);
 
             self.set_speeds(
@@ -530,7 +517,7 @@ impl Drivetrain {
                 SwerveControlStyle::RobotOriented,
             );
 
-            return if error_ty.abs() < 2. && error_tx.abs() < 2. && error_yaw.abs() < 0.2 {
+            return if error_ty.abs() < TX_ACCEPTABLE_ERROR && error_tx.abs() < TY_ACCEPTABLE_ERROR && error_yaw.abs() < YAW_ACCEPTABLE_ERROR {
                 println!("Drivetrain at target");
                 true
             } else {
diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs
index 4108d35..3ee0c9c 100644
--- a/src/subsystems/elevator.rs
+++ b/src/subsystems/elevator.rs
@@ -29,6 +29,7 @@ impl Elevator {
     pub fn new() -> Self {
         let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string()));
         let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config
+        println!("left pos: {} right pos: {} diff: {}",right.get_position(),left.get_position(),right.get_position() - left.get_position());
 
         //right.follow(&left, true);
 
@@ -70,13 +71,14 @@ impl Elevator {
 
         // current implementation is to just set the control mode and call this function every frame
         self.right.set(ControlMode::MotionMagic, target_position);
-        self.left.set(ControlMode::MotionMagic, target_position);
+        self.left.follow(&self.right, true);
 
+        println!("left pos: {} right pos: {} diff: {}",self.right.get_position(),self.left.get_position(),self.right.get_position() - self.left.get_position());
         if (target_position - self.right.get_position()).abs() < 0.5 {
             println!("Elevator at target");
             true
         } else {
-            println!("Elevator not at target {}", (target_position - self.right.get_position()).abs());
+           // println!("Elevator not at target {}", (target_position - self.right.get_position()).abs());
             false
         }