From 75354e2df79669aa420f29938be27dd68fecd44c Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Thu, 20 Mar 2025 19:59:26 -0500
Subject: [PATCH 01/14] wonky L4 setpoin

---
 src/constants.rs | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index ce4dd16..97f0ec4 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -22,6 +22,9 @@ pub mod robotmap {
         pub const BR_DRIVE: i32 = 7;
         pub const BR_TURN: i32 = 8;
         pub const BR_ENCODER: i32 = 16;
+
+        pub const RIGHT_LINEUP_LASER:i32 = 18;
+        pub const LEFT_LINEUP_LASER:i32 = 19;
     }
 
     pub mod elevator {
@@ -32,7 +35,7 @@ pub mod robotmap {
     pub mod indexer {
         pub const MOTOR: i32 = 12;
         pub const LASER_CAN: i32 = 0; // Cant save can id
-
+        pub const INDEXER_775_MOTOR: i32 = 17;
     }
 
     pub mod climber {
@@ -119,7 +122,7 @@ pub mod elevator {
     pub const BOTTOM: f64 = 0.25; // unit is rotations
     pub const L2: f64 = 1.5; // unit is rotations
     pub const L3: f64 = 13.; // unit is rotations
-    pub const L4: f64 = 39.5; // unit is rotations
+    pub const L4: f64 = 45.15; // 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
 }

From 24234394d5fd28e1f640a17b096a25ed0350fd7b Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Thu, 20 Mar 2025 20:00:13 -0500
Subject: [PATCH 02/14] canrange in drivetrain, post to telemetry

---
 src/subsystems/drivetrain.rs | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 0ce6297..a66a619 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -6,7 +6,7 @@ use std::net::{IpAddr, Ipv4Addr, SocketAddr};
 use std::ops::{Add, Sub};
 use std::time::Duration;
 
-use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon};
+use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon, CanRange};
 
 use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, 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::*;
@@ -73,6 +73,9 @@ pub struct Drivetrain {
     br_turn: Talon,
     br_encoder: CanCoder,
 
+    right_laser: CanRange, //from robot perspective
+    left_laser: CanRange,
+
     kinematics: Swerve,
     pub odometry: Odometry,
 
@@ -174,6 +177,9 @@ impl Drivetrain {
             br_turn,
             br_encoder,
 
+            right_laser: CanRange::new(RIGHT_LINEUP_LASER, Some("can0".to_owned())),
+            left_laser: CanRange::new(LEFT_LINEUP_LASER, Some("can0".to_owned())),
+
             kinematics: Swerve::rectangle(Length::new::<inch>(21.5), Length::new::<inch>(21.5)),
             odometry: Odometry::new(),
 
@@ -287,6 +293,9 @@ impl Drivetrain {
                 .get::<meter>(),
         )
         .await;
+        Telemetry::put_number("left laser", self.left_laser.get_distance().get::<meter>()).await;
+        Telemetry::put_number("right laser", self.right_laser.get_distance().get::<meter>()).await;
+
     }
 
     pub fn update_odo_absolute(&mut self, pose: Vector2<Length>) {

From 4a7ab00300707bf18335a22f32a4e814eeb84411 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Thu, 20 Mar 2025 20:41:04 -0500
Subject: [PATCH 03/14] wonky climber

---
 src/constants.rs          | 11 ++++-----
 src/main.rs               | 42 ++++++++--------------------------
 src/subsystems/climber.rs | 47 +++++----------------------------------
 3 files changed, 21 insertions(+), 79 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 97f0ec4..e51b278 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -39,8 +39,7 @@ pub mod robotmap {
     }
 
     pub mod climber {
-        pub const RAISE: i32 = 1;
-        pub const GRAB: i32 = 0;
+        pub const CLIMBER_MOTOR_ID: i32 = 20;
     }
 
     pub mod led {
@@ -70,7 +69,7 @@ pub mod drivetrain {
     use std::f64::consts::PI;
 
     pub const FR_OFFSET_DEGREES: f64 = 0.081299 * 360.;
-    pub const FL_OFFSET_DEGREES: f64 = 0.079102 * 360.;
+    pub const FL_OFFSET_DEGREES: f64 = 0.0983 * 360.;
     pub const BR_OFFSET_DEGREES: f64 = -0.056641 * 360.;
     pub const BL_OFFSET_DEGREES: f64 = 0.170898 * 360.;
 
@@ -134,6 +133,10 @@ pub mod indexer {
     pub const L3_SPEED: f64 = -0.4;
     pub const L4_SPEED: f64 = -0.4;
 }
+pub mod climber {
+    pub const CLIMB_SPEED: f64 = 0.3;
+    pub const FALL_SPEED: f64 = -0.3;
+}
 pub mod joystick_map {
     // Joystick IDs (set in driver station)
     pub const RIGHT_DRIVE: i32 = 0;
@@ -147,8 +150,6 @@ pub mod joystick_map {
     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/main.rs b/src/main.rs
index b774c5d..fec8127 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -15,10 +15,11 @@ use frcrs::telemetry::Telemetry;
 use tokio::task;
 use tokio::task::{AbortHandle, spawn_local};
 use tokio::time::sleep;
-use RobotCode2025::constants::joystick_map::{CLIMB, CLIMB_FALL, CLIMBER_GRAB, CLIMBER_RAISE, INTAKE, LINEUP_LEFT, LINEUP_RIGHT, SCORE_L2, SCORE_L3, SCORE_L4, WHEELS_ZERO};
+use RobotCode2025::constants::joystick_map::{CLIMB, CLIMB_FALL, INTAKE, LINEUP_LEFT, LINEUP_RIGHT, SCORE_L2, SCORE_L3, SCORE_L4, WHEELS_ZERO};
 use RobotCode2025::container::control_drivetrain;
 use RobotCode2025::{constants, Ferris, score, TeleopState};
 use RobotCode2025::auto::Auto;
+use RobotCode2025::constants::climber::{CLIMB_SPEED, FALL_SPEED};
 use RobotCode2025::constants::indexer::INTAKE_SPEED;
 use RobotCode2025::subsystems::{Climber, ElevatorPosition, LineupSide};
 
@@ -227,38 +228,13 @@ async fn teleop(robot: &mut Ferris) {
         }
     }
 
-    if robot.controllers.right_drive.get(CLIMB) {
-        if robot.climb_handle.is_none() {
-            let f = robot.clone();
-            let climb_task = Climber::climb(f);
-            let handle = spawn_local(climb_task).abort_handle();
-            robot.climb_handle = Some(handle);
-        }
-    } else if robot.controllers.right_drive.get(CLIMB_FALL) {
-        if let Some(handle) = robot.climb_handle.take() {
-            handle.abort();
-        }
-
-        if let Ok(mut climber) = robot.climber.try_borrow_mut() {
-            climber.fall()
-        }
-    } else {
-        if let Some(handle) = robot.climb_handle.take() {
-            handle.abort();
-        }
-
-        if let Ok(mut climber) = robot.climber.try_borrow_mut() {
-            if robot.controllers.right_drive.get(CLIMBER_RAISE) {
-                climber.set_raise(true);
-            } else {
-                climber.set_raise(false);
-            }
-
-            if robot.controllers.right_drive.get(CLIMBER_GRAB) {
-                climber.set_grab(true);
-            } else {
-                climber.set_grab(false);
-            }
+    if let Ok(climber) = robot.climber.try_borrow_mut() {
+        if robot.controllers.right_drive.get(CLIMB) {
+            climber.set(CLIMB_SPEED);
+        } else if robot.controllers.right_drive.get(CLIMB_FALL) {
+            climber.set(FALL_SPEED);
+        } else {
+            climber.set(0.);
         }
     }
 }
\ No newline at end of file
diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs
index 989113f..f4e0b5b 100644
--- a/src/subsystems/climber.rs
+++ b/src/subsystems/climber.rs
@@ -2,11 +2,12 @@ use crate::constants::robotmap::climber::*;
 use crate::Ferris;
 use frcrs::solenoid::{ModuleType, Solenoid};
 use std::time::Duration;
+use frcrs::ctre::{ControlMode, Talon};
 use tokio::time::sleep;
+use crate::constants::robotmap;
 
 pub struct Climber {
-    raise: Solenoid,
-    grab: Solenoid,
+    motor: Talon
 }
 
 impl Default for Climber {
@@ -18,47 +19,11 @@ impl Default for Climber {
 impl Climber {
     pub fn new() -> Self {
         Self {
-            raise: Solenoid::new(ModuleType::CTRE, RAISE),
-            grab: Solenoid::new(ModuleType::CTRE, GRAB),
+            motor: Talon::new(CLIMBER_MOTOR_ID, Some("can0".to_string()))
         }
     }
 
-    pub fn toggle_raise(&self) {
-        self.raise.toggle();
-    }
-
-    pub fn set_raise(&self, engaged: bool) {
-        self.raise.set(engaged);
-    }
-
-    pub fn toggle_grab(&self) {
-        self.grab.toggle();
-    }
-
-    pub fn set_grab(&self, engaged: bool) {
-        self.grab.set(!engaged);
-    }
-
-    pub async fn climb(ferris: Ferris) {
-        if let Ok(climber) = ferris.climber.try_borrow_mut() {
-            climber.set_raise(true);
-            sleep(Duration::from_secs_f64(1.75)).await;
-            climber.set_grab(true);
-            sleep(Duration::from_secs_f64(0.25)).await;
-            climber.set_raise(false);
-        }
-    }
-
-    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;
-        self.set_grab(false);
-        sleep(Duration::from_secs_f64(0.25)).await;
-        self.set_raise(false);
+    pub fn set(&self, speed: f64){
+        self.motor.set(ControlMode::Percent, speed);
     }
 }

From 7ac270b5d762507a9421140a3dc2e8f549eb77b2 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sat, 22 Mar 2025 11:33:05 -0500
Subject: [PATCH 04/14] debouncer & tuning

---
 src/constants.rs             | 10 ++++--
 src/subsystems/drivetrain.rs | 65 ++++++++++++++++++++++++++++++++++--
 2 files changed, 70 insertions(+), 5 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index e51b278..1725975 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -55,12 +55,12 @@ pub const HALF_FIELD_LENGTH_METERS: f64 = 8.05 / 2.;
 pub mod vision {
     use nalgebra::Vector2;
 
-    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -34.5; //Last measured: -37.0
+    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -36.4; //Last measured: -37.3 past -37.0
     pub const LIMELIGHT_UPPER_YAW_DEGREES: f64 = 90.; // Counterclockwise positive
     pub const LIMELIGHT_UPPER_HEIGHT_INCHES: f64 = 20.92;
     pub const ROBOT_CENTER_TO_LIMELIGHT_UPPER_INCHES: Vector2<f64> = Vector2::new(11.118, 10.352);
 
-    // Increase distance by 10% for every 20 degrees of absolute value of tx
+    // Increase distance by 13.5% for every 20 degrees of absolute value of tx
     // Set this to 0 for new robots
     pub const TX_FUDGE_FACTOR: f64 = 0.135 / 20.;
 }
@@ -69,7 +69,7 @@ pub mod drivetrain {
     use std::f64::consts::PI;
 
     pub const FR_OFFSET_DEGREES: f64 = 0.081299 * 360.;
-    pub const FL_OFFSET_DEGREES: f64 = 0.0983 * 360.;
+    pub const FL_OFFSET_DEGREES: f64 = 0.0942 * 360.;
     pub const BR_OFFSET_DEGREES: f64 = -0.056641 * 360.;
     pub const BL_OFFSET_DEGREES: f64 = 0.170898 * 360.;
 
@@ -106,6 +106,9 @@ pub mod drivetrain {
     pub const LINEUP_DRIVE_KI: f64 = 10.;
     pub const LINEUP_DRIVE_KD: f64 = 10.;
     pub const LINEUP_DRIVE_IE: f64 = 0.25;
+
+    pub const CANRANGE_DEBOUNCE_TIME_SECONDS: f64 = 0.02;
+    pub const REEF_SENSOR_TARGET_DISTANCE_METERS: f64 = 0.381;
 }
 pub mod pose_estimation {
     pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.000001;
@@ -127,6 +130,7 @@ pub mod elevator {
 }
 pub mod indexer {
     pub const LASER_TRIP_DISTANCE_MM: i32 = 2;
+    pub const LASERCAN_DEBOUNCE_TIME_SECONDS: f64 = 0.3;
     pub const INTAKE_SPEED: f64 = -0.25;
     pub const BOTTOM_SPEED: f64 = -0.35;
     pub const L2_SPEED: f64 = -0.4;
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index a66a619..e27665b 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -1,3 +1,4 @@
+use std::cmp::PartialEq;
 use std::collections::HashMap;
 use frcrs::alliance_station;
 use std::f64::consts::PI;
@@ -8,7 +9,7 @@ use std::time::Duration;
 
 use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon, CanRange};
 
-use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, TARGET_TX_LEFT, TARGET_TX_RIGHT, TARGET_TY_LEFT, TARGET_TY_RIGHT, TX_ACCEPTABLE_ERROR, TY_ACCEPTABLE_ERROR, YAW_ACCEPTABLE_ERROR};
+use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, CANRANGE_DEBOUNCE_TIME_SECONDS, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, REEF_SENSOR_TARGET_DISTANCE_METERS, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, 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};
@@ -54,6 +55,58 @@ pub struct LineupLocation {
     forward_distance: Length,
 }
 
+#[derive(Copy, Clone)]
+pub enum DebounceType {
+    RISING,
+    FALLING,
+    BOTH
+}
+
+/// https://docs.wpilib.org/en/stable/docs/software/advanced-controls/filters/debouncer.html
+/// but in rust
+pub struct Debouncer {
+    bounce_time: Duration,
+    debounce_type: DebounceType,
+    baseline: bool,
+    prev_time: Instant,
+}
+
+impl Debouncer {
+    pub fn new(bounce_time: Duration, debounce_type: DebounceType) -> Debouncer {
+        Debouncer {
+            bounce_time,
+            debounce_type,
+            baseline: match debounce_type {
+                DebounceType::RISING => false,
+                DebounceType::FALLING => true,
+                DebounceType::BOTH => false,
+            },
+            prev_time: Instant::now(),
+        }
+    }
+    fn reset_timer(&mut self) {
+        self.prev_time = Instant::now();
+    }
+    fn has_elapsed(&self) -> bool {
+        self.prev_time.elapsed() >= self.bounce_time
+    }
+    /// copied line for line from wpilib
+    pub fn calculate(&mut self, input: bool) -> bool{
+        if input == self.baseline {
+            self.reset_timer()
+        }
+        if self.has_elapsed() {
+            if matches!(self.debounce_type, DebounceType::BOTH) {
+                self.baseline = input;
+                self.reset_timer();
+            }
+            input
+        } else {
+            self.baseline
+        }
+    }
+}
+
 pub struct Drivetrain {
     pigeon: Pigeon,
 
@@ -75,6 +128,8 @@ pub struct Drivetrain {
 
     right_laser: CanRange, //from robot perspective
     left_laser: CanRange,
+    right_laser_debouncer: Debouncer,
+    left_laser_debouncer: Debouncer,
 
     kinematics: Swerve,
     pub odometry: Odometry,
@@ -179,6 +234,8 @@ impl Drivetrain {
 
             right_laser: CanRange::new(RIGHT_LINEUP_LASER, Some("can0".to_owned())),
             left_laser: CanRange::new(LEFT_LINEUP_LASER, Some("can0".to_owned())),
+            right_laser_debouncer: Debouncer::new(Duration::from_secs_f64(CANRANGE_DEBOUNCE_TIME_SECONDS), DebounceType::RISING),
+            left_laser_debouncer: Debouncer::new(Duration::from_secs_f64(CANRANGE_DEBOUNCE_TIME_SECONDS), DebounceType::RISING),
 
             kinematics: Swerve::rectangle(Length::new::<inch>(21.5), Length::new::<inch>(21.5)),
             odometry: Odometry::new(),
@@ -265,7 +322,7 @@ impl Drivetrain {
         ]
     }
 
-    pub async fn post_odo(&self) {
+    pub async fn post_odo(&mut self) {
         Telemetry::put_number(
             "odo_x",
             self.odometry
@@ -295,6 +352,10 @@ impl Drivetrain {
         .await;
         Telemetry::put_number("left laser", self.left_laser.get_distance().get::<meter>()).await;
         Telemetry::put_number("right laser", self.right_laser.get_distance().get::<meter>()).await;
+        let left_laser_tripped: String = if self.left_laser_debouncer.calculate(self.left_laser.get_distance().get::<meter>() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.left_laser.get_distance().get::<meter>() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() };
+        Telemetry::put_string("left_laser_tripped", left_laser_tripped).await;
+        let left_laser_tripped: String = if self.right_laser_debouncer.calculate(self.right_laser.get_distance().get::<meter>() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.right_laser.get_distance().get::<meter>() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() };
+        Telemetry::put_string("left_laser_tripped", left_laser_tripped).await;
 
     }
 

From 399fb28c549a21944f394fbb26ec36dd3e1c2f38 Mon Sep 17 00:00:00 2001
From: TheSnowHatHero <thesnowhathero@gmail.com>
Date: Sat, 22 Mar 2025 18:25:47 -0500
Subject: [PATCH 05/14] auto adjustment and such

---
 src/auto/mod.rs              | 14 +++++++-------
 src/constants.rs             |  8 ++++----
 src/lib.rs                   |  2 +-
 src/main.rs                  |  3 +--
 src/subsystems/drivetrain.rs | 12 ++++++------
 src/subsystems/indexer.rs    | 16 +++++++++++-----
 6 files changed, 30 insertions(+), 25 deletions(-)

diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 54f5757..64e1104 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -144,7 +144,7 @@ pub async fn async_score(
     };
     indexer.set_speed(indexer_speed);
 
-    wait(|| indexer.get_laser_dist() > LASER_TRIP_DISTANCE_MM || indexer.get_laser_dist() == -1).await;
+    wait(|| !indexer.is_laser_tripped()).await;
 
     sleep(Duration::from_secs_f64(0.2)).await;
     indexer.stop();
@@ -242,14 +242,14 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
         elevator.run_to_target_trapezoid();
 
         indexer.set_speed(INTAKE_SPEED);
-        wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
+        wait(|| indexer.is_laser_tripped()).await;
         indexer.stop();
 
         elevator.set_target(ElevatorPosition::L4);
         elevator.run_to_target_trapezoid();
     });
 
-    let _ = timeout(Duration::from_secs_f64(1.5), async {
+    let _ = timeout(Duration::from_secs_f64(0.5), async {
         loop {
             drivetrain.update_limelight().await;
             drivetrain.post_odo().await;
@@ -275,7 +275,7 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
 
         indexer.set_speed(INTAKE_SPEED);
 
-        wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
+        wait(|| indexer.is_laser_tripped()).await;
 
         indexer.stop();
     });
@@ -286,7 +286,7 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
         elevator.run_to_target_trapezoid();
     });
 
-    let _ = timeout(Duration::from_secs_f64(1.25), async {
+    let _ = timeout(Duration::from_secs_f64(0.5), async {
         loop {
             drivetrain.update_limelight().await;
             sleep(Duration::from_millis(20)).await;
@@ -370,7 +370,7 @@ async fn blue_mid_left_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::
 
         indexer.set_speed(INTAKE_SPEED);
 
-        wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
+        wait(|| indexer.is_laser_tripped()).await;
 
         indexer.stop();
     });
@@ -427,7 +427,7 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
         elevator.run_to_target_trapezoid();
 
         indexer.set_speed(INTAKE_SPEED);
-        wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
+        wait(|| indexer.is_laser_tripped()).await;
         indexer.stop();
 
         elevator.set_target(ElevatorPosition::L4);
diff --git a/src/constants.rs b/src/constants.rs
index 1725975..913fe07 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -122,18 +122,18 @@ pub mod pose_estimation {
 }
 pub mod elevator {
     pub const BOTTOM: f64 = 0.25; // unit is rotations
-    pub const L2: f64 = 1.5; // unit is rotations
+    pub const L2: f64 = 0.05; // unit is rotations
     pub const L3: f64 = 13.; // unit is rotations
-    pub const L4: f64 = 45.15; // unit is rotations
+    pub const L4: f64 = 42.9; // 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
 }
 pub mod indexer {
     pub const LASER_TRIP_DISTANCE_MM: i32 = 2;
-    pub const LASERCAN_DEBOUNCE_TIME_SECONDS: f64 = 0.3;
+    pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08;
     pub const INTAKE_SPEED: f64 = -0.25;
     pub const BOTTOM_SPEED: f64 = -0.35;
-    pub const L2_SPEED: f64 = -0.4;
+    pub const L2_SPEED: f64 = -0.2;
     pub const L3_SPEED: f64 = -0.4;
     pub const L4_SPEED: f64 = -0.4;
 }
diff --git a/src/lib.rs b/src/lib.rs
index 8ca7104..19bbdab 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -343,7 +343,7 @@ pub fn score(
     let elevator_at_target = elevator.run_to_target_trapezoid();
 
     if elevator_at_target && drivetrain_aligned {
-        if indexer.get_laser_dist() < constants::indexer::LASER_TRIP_DISTANCE_MM {
+        if indexer.is_laser_tripped() {
             let indexer_speed = match elevator_position {
                 ElevatorPosition::Bottom => BOTTOM_SPEED,
                 ElevatorPosition::L2 => L2_SPEED,
diff --git a/src/main.rs b/src/main.rs
index fec8127..4e59cbf 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -209,8 +209,7 @@ async fn teleop(robot: &mut Ferris) {
                     elevator.set_target(ElevatorPosition::Bottom);
                     elevator.run_to_target_trapezoid();
 
-                    if indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM
-                        || indexer.get_laser_dist() == -1
+                    if !indexer.is_laser_tripped()
                     {
                         indexer.set_speed(INTAKE_SPEED);
                     } else {
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index e27665b..8e42837 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -208,10 +208,10 @@ impl Drivetrain {
         /// default is:
         // side_distance: Length::new::<inch>(13. / 2.),
         // forward_distance: Length::new::<inch>(16.275),
-        lineup_locations.insert(17, LineupLocation {
-            side_distance: Length::new::<inch>(13. / 2.),
-            forward_distance: Length::new::<inch>(16.),
-        });
+        // lineup_locations.insert(17, LineupLocation {
+        //     side_distance: Length::new::<inch>(13. / 2.),
+        //     forward_distance: Length::new::<inch>(16.),
+        // });
 
         Self {
             pigeon: Pigeon::new(PIGEON, Some("can0".to_owned())),
@@ -353,9 +353,9 @@ impl Drivetrain {
         Telemetry::put_number("left laser", self.left_laser.get_distance().get::<meter>()).await;
         Telemetry::put_number("right laser", self.right_laser.get_distance().get::<meter>()).await;
         let left_laser_tripped: String = if self.left_laser_debouncer.calculate(self.left_laser.get_distance().get::<meter>() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.left_laser.get_distance().get::<meter>() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() };
-        Telemetry::put_string("left_laser_tripped", left_laser_tripped).await;
+        Telemetry::put_string("left laser tripped", left_laser_tripped).await;
         let left_laser_tripped: String = if self.right_laser_debouncer.calculate(self.right_laser.get_distance().get::<meter>() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.right_laser.get_distance().get::<meter>() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() };
-        Telemetry::put_string("left_laser_tripped", left_laser_tripped).await;
+        Telemetry::put_string("right laser tripped", left_laser_tripped).await;
 
     }
 
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index dba0857..0d8632b 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -7,11 +7,13 @@ use std::rc::Rc;
 use std::time::Duration;
 use std::time::Instant;
 use tokio::time::sleep;
-use crate::constants::indexer::INTAKE_SPEED;
+use crate::constants::indexer::{INDEXER_LASER_DEBOUNCE_TIME_SECONDS, INTAKE_SPEED, LASER_TRIP_DISTANCE_MM};
+use crate::subsystems::{DebounceType, Debouncer};
 
 pub struct Indexer {
     motor: Talon,
     laser_can: LaserCan,
+    debouncer: Debouncer,
 }
 
 impl Default for Indexer {
@@ -24,8 +26,9 @@ impl Indexer {
     pub fn new() -> Self {
         let motor = Talon::new(robotmap::indexer::MOTOR, None);
         let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN);
+        let debouncer = Debouncer::new(Duration::from_secs_f64(INDEXER_LASER_DEBOUNCE_TIME_SECONDS), DebounceType::RISING);
 
-        Self { motor, laser_can }
+        Self { motor, laser_can, debouncer }
     }
 
     pub fn set_speed(&self, speed: f64) {
@@ -33,9 +36,8 @@ impl Indexer {
     }
 
     pub async fn intake_coral(indexer: Rc<RefCell<Indexer>>) {
-        if let Ok(indexer) = indexer.try_borrow_mut() {
-            while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM
-                || indexer.laser_can.get_measurement() == -1
+        if let Ok(mut indexer) = indexer.try_borrow_mut() {
+            while !indexer.is_laser_tripped()
             {
                 println!("Dist: {}", indexer.get_laser_dist());
                 indexer.set_speed(INTAKE_SPEED);
@@ -49,6 +51,10 @@ impl Indexer {
         self.laser_can.get_measurement()
     }
 
+    pub fn is_laser_tripped(&mut self) -> bool {
+        self.debouncer.calculate(self.get_laser_dist() < LASER_TRIP_DISTANCE_MM && self.get_laser_dist() != -1)
+    }
+
     pub fn stop(&self) {
         self.motor.stop();
     }

From 2b374b8dad5dcd6fb8a42bb9f418147a19e5bf2e Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Thu, 27 Mar 2025 13:57:06 -0500
Subject: [PATCH 06/14] channges from weekend

---
 src/auto/mod.rs              |  2 ++
 src/constants.rs             | 15 ++++++++-------
 src/lib.rs                   |  9 +++------
 src/main.rs                  | 13 +++++++++----
 src/subsystems/climber.rs    |  9 +++++++++
 src/subsystems/drivetrain.rs | 17 +++++++++--------
 src/subsystems/elevator.rs   | 27 +++++++++++++++------------
 src/subsystems/indexer.rs    |  2 +-
 8 files changed, 56 insertions(+), 38 deletions(-)

diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 64e1104..1aad7ee 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -16,6 +16,7 @@ use uom::si::f64::Angle;
 
 use crate::subsystems::{Drivetrain, Elevator, ElevatorPosition, Indexer, LineupSide};
 use crate::{constants, score, Ferris};
+use crate::constants::elevator::L3_ALGAE;
 use crate::constants::indexer::{BOTTOM_SPEED, INTAKE_SPEED, L2_SPEED, L3_SPEED, L4_SPEED, LASER_TRIP_DISTANCE_MM};
 
 #[derive(Serialize, Deserialize)]
@@ -141,6 +142,7 @@ pub async fn async_score(
         ElevatorPosition::L2 => L2_SPEED,
         ElevatorPosition::L3 => L3_SPEED,
         ElevatorPosition::L4 => L4_SPEED,
+        ElevatorPosition::L3Algae => L3_SPEED
     };
     indexer.set_speed(indexer_speed);
 
diff --git a/src/constants.rs b/src/constants.rs
index 913fe07..dd5a617 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -55,14 +55,14 @@ pub const HALF_FIELD_LENGTH_METERS: f64 = 8.05 / 2.;
 pub mod vision {
     use nalgebra::Vector2;
 
-    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -36.4; //Last measured: -37.3 past -37.0
+    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -35.5; //Last measured 3/27 @ 9:27: -36.8, -37.3, -37.0
     pub const LIMELIGHT_UPPER_YAW_DEGREES: f64 = 90.; // Counterclockwise positive
     pub const LIMELIGHT_UPPER_HEIGHT_INCHES: f64 = 20.92;
     pub const ROBOT_CENTER_TO_LIMELIGHT_UPPER_INCHES: Vector2<f64> = Vector2::new(11.118, 10.352);
 
     // Increase distance by 13.5% for every 20 degrees of absolute value of tx
     // Set this to 0 for new robots
-    pub const TX_FUDGE_FACTOR: f64 = 0.135 / 20.;
+    pub const TX_FUDGE_FACTOR: f64 = 0.165 / 20.;
 }
 
 pub mod drivetrain {
@@ -77,7 +77,7 @@ pub mod drivetrain {
 
     pub const SWERVE_TURN_KP: f64 = 0.6;
 
-    pub const SWERVE_ROTATIONS_TO_INCHES: f64 = (1. / 6.75) * (3.65 * PI);
+    pub const SWERVE_ROTATIONS_TO_INCHES: f64 = (1. / 6.75) * (3.75 * PI);
     pub const SWERVE_TURN_RATIO: f64 = 12.8;
 
     pub const SWERVE_DRIVE_KP: f64 = 0.7;
@@ -102,7 +102,7 @@ pub mod drivetrain {
     pub const TY_ACCEPTABLE_ERROR: f64 = 1.8;
     pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02;
 
-    pub const LINEUP_DRIVE_KP: f64 = 1.;
+    pub const LINEUP_DRIVE_KP: f64 = 1.1;
     pub const LINEUP_DRIVE_KI: f64 = 10.;
     pub const LINEUP_DRIVE_KD: f64 = 10.;
     pub const LINEUP_DRIVE_IE: f64 = 0.25;
@@ -123,8 +123,9 @@ pub mod pose_estimation {
 pub mod elevator {
     pub const BOTTOM: f64 = 0.25; // unit is rotations
     pub const L2: f64 = 0.05; // unit is rotations
-    pub const L3: f64 = 13.; // unit is rotations
+    pub const L3: f64 = 12.5; // unit is rotations
     pub const L4: f64 = 42.9; // unit is rotations
+    pub const L3_ALGAE: f64 = 13.; // 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
 }
@@ -133,12 +134,12 @@ pub mod indexer {
     pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08;
     pub const INTAKE_SPEED: f64 = -0.25;
     pub const BOTTOM_SPEED: f64 = -0.35;
-    pub const L2_SPEED: f64 = -0.2;
+    pub const L2_SPEED: f64 = -0.4;
     pub const L3_SPEED: f64 = -0.4;
     pub const L4_SPEED: f64 = -0.4;
 }
 pub mod climber {
-    pub const CLIMB_SPEED: f64 = 0.3;
+    pub const CLIMB_SPEED: f64 = 0.5;
     pub const FALL_SPEED: f64 = -0.3;
 }
 pub mod joystick_map {
diff --git a/src/lib.rs b/src/lib.rs
index 19bbdab..f40249e 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -30,6 +30,7 @@ use tokio::runtime::Handle;
 use tokio::task::{spawn_local, AbortHandle};
 use tokio::time::sleep;
 use uom::si::angle::degree;
+use crate::constants::elevator::L3_ALGAE;
 use crate::constants::indexer::{BOTTOM_SPEED, L2_SPEED, L3_SPEED, L4_SPEED};
 
 #[derive(Clone)]
@@ -317,12 +318,7 @@ 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,
-        };
+        let target_position = elevator.get_target().get_position();
         //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 {
@@ -349,6 +345,7 @@ pub fn score(
                 ElevatorPosition::L2 => L2_SPEED,
                 ElevatorPosition::L3 => L3_SPEED,
                 ElevatorPosition::L4 => L4_SPEED,
+                ElevatorPosition::L3Algae => L3_SPEED,
             };
             indexer.set_speed(indexer_speed);
         } else {
diff --git a/src/main.rs b/src/main.rs
index 4e59cbf..4265339 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -20,7 +20,7 @@ use RobotCode2025::container::control_drivetrain;
 use RobotCode2025::{constants, Ferris, score, TeleopState};
 use RobotCode2025::auto::Auto;
 use RobotCode2025::constants::climber::{CLIMB_SPEED, FALL_SPEED};
-use RobotCode2025::constants::indexer::INTAKE_SPEED;
+use RobotCode2025::constants::indexer::{INTAKE_SPEED, L3_SPEED};
 use RobotCode2025::subsystems::{Climber, ElevatorPosition, LineupSide};
 
 fn main() {
@@ -43,7 +43,7 @@ fn main() {
 
         Telemetry::put_selector("auto chooser", Auto::names()).await;
 
-        SmartDashboard::start_camera_server();
+        // SmartDashboard::start_camera_server();
 
         let mut last_loop = Instant::now();
 
@@ -184,7 +184,12 @@ async fn teleop(robot: &mut Ferris) {
                     false
                 };
 
-                if robot.controllers.left_drive.get(SCORE_L2) {
+                if robot.controllers.left_drive.get_pov() != -1 {
+                    elevator.set_target(ElevatorPosition::L3Algae);
+                    elevator.run_to_target_trapezoid();
+                    
+                    indexer.set_speed(L3_SPEED);
+                } else if robot.controllers.left_drive.get(SCORE_L2) {
                     score(
                         drivetrain_aligned,
                         &mut elevator,
@@ -229,7 +234,7 @@ async fn teleop(robot: &mut Ferris) {
 
     if let Ok(climber) = robot.climber.try_borrow_mut() {
         if robot.controllers.right_drive.get(CLIMB) {
-            climber.set(CLIMB_SPEED);
+            climber.climb();
         } else if robot.controllers.right_drive.get(CLIMB_FALL) {
             climber.set(FALL_SPEED);
         } else {
diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs
index f4e0b5b..499f7f1 100644
--- a/src/subsystems/climber.rs
+++ b/src/subsystems/climber.rs
@@ -4,6 +4,7 @@ use frcrs::solenoid::{ModuleType, Solenoid};
 use std::time::Duration;
 use frcrs::ctre::{ControlMode, Talon};
 use tokio::time::sleep;
+use crate::constants::climber::CLIMB_SPEED;
 use crate::constants::robotmap;
 
 pub struct Climber {
@@ -26,4 +27,12 @@ impl Climber {
     pub fn set(&self, speed: f64){
         self.motor.set(ControlMode::Percent, speed);
     }
+    
+    pub fn climb(&self) {
+        if self.motor.get_position() < 330. {
+            self.set(CLIMB_SPEED);
+        } else {
+            self.set(0.);
+        }
+    }
 }
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 8e42837..2cb3a8a 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -8,7 +8,7 @@ use std::ops::{Add, Sub};
 use std::time::Duration;
 
 use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon, CanRange};
-
+use frcrs::redux::CanAndGyro;
 use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, CANRANGE_DEBOUNCE_TIME_SECONDS, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, REEF_SENSOR_TARGET_DISTANCE_METERS, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, 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};
@@ -108,7 +108,7 @@ impl Debouncer {
 }
 
 pub struct Drivetrain {
-    pigeon: Pigeon,
+    gyro: CanAndGyro,
 
     fr_drive: Talon,
     fr_turn: Talon,
@@ -214,7 +214,7 @@ impl Drivetrain {
         // });
 
         Self {
-            pigeon: Pigeon::new(PIGEON, Some("can0".to_owned())),
+            gyro: CanAndGyro::new(PIGEON),
 
             fr_drive,
             fr_turn,
@@ -587,7 +587,7 @@ impl Drivetrain {
     }
 
     pub fn get_angle(&self) -> Angle {
-        Angle::new::<radian>(-self.pigeon.get_rotation().z + PIGEON_OFFSET)
+        Angle::new::<revolution>(-self.gyro.get_angle() + PIGEON_OFFSET)
     }
 
     pub fn get_offset(&self) -> Angle {
@@ -607,9 +607,9 @@ impl Drivetrain {
         Angle::new::<degree>(difference)
     }
 
-    pub fn reset_angle(&self) {
-        self.pigeon.reset();
-    }
+    // pub fn reset_angle(&self) {
+    //     self.gyro.reset();
+    // }
 
     pub fn reset_heading(&mut self) {
         println!("Resetting heading: {}", self.get_offset_wrapped().get::<degree>());
@@ -649,7 +649,7 @@ impl Drivetrain {
 
             // Give KP boost when close
             if error_position.magnitude().abs() < 0.15 {
-                error_position *= 2.;
+                error_position *= 2.25;
             }
 
             let mut speed = error_position;
@@ -735,6 +735,7 @@ impl Drivetrain {
             ElevatorPosition::L2 => Length::new::<inch>(-9.),
             ElevatorPosition::L3 => Length::new::<inch>(-9.),
             ElevatorPosition::L4 => Length::new::<inch>(-9.),
+            ElevatorPosition::L3Algae => Length::new::<inch>(-9.),
         }; //theoretical is -11.0
 
         let side_multiplier = match side {
diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs
index 40381b2..673ac78 100644
--- a/src/subsystems/elevator.rs
+++ b/src/subsystems/elevator.rs
@@ -18,6 +18,19 @@ pub enum ElevatorPosition {
     L2,
     L3,
     L4,
+    L3Algae,
+}
+
+impl ElevatorPosition {
+    pub fn get_position(&self) -> f64 {
+        match self {
+            ElevatorPosition::Bottom => elevator::BOTTOM,
+            ElevatorPosition::L2 => elevator::L2,
+            ElevatorPosition::L3 => elevator::L3,
+            ElevatorPosition::L4 => elevator::L4,
+            ElevatorPosition::L3Algae => elevator::L3_ALGAE
+        }
+    }
 }
 
 impl Default for Elevator {
@@ -65,12 +78,7 @@ impl Elevator {
     /// Those configuration files have been saved to Documents on the driver station laptop.
     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,
-            ElevatorPosition::L2 => elevator::L2,
-            ElevatorPosition::L3 => elevator::L3,
-            ElevatorPosition::L4 => elevator::L4,
-        };
+        let target_position = self.get_target().get_position();
 
         // current implementation is to just set the control mode and call this function every frame
         self.right.set(ControlMode::MotionMagic, target_position);
@@ -95,12 +103,7 @@ impl Elevator {
     }
 
     pub async fn run_to_target_trapezoid_async(&mut self) {
-        let target_position = match self.get_target() {
-            ElevatorPosition::Bottom => elevator::BOTTOM,
-            ElevatorPosition::L2 => elevator::L2,
-            ElevatorPosition::L3 => elevator::L3,
-            ElevatorPosition::L4 => elevator::L4,
-        };
+        let target_position = self.get_target().get_position();
 
         self.right.set(ControlMode::MotionMagic, target_position);
         self.left.follow(&self.right, true);
diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs
index 0d8632b..e7f05c2 100644
--- a/src/subsystems/indexer.rs
+++ b/src/subsystems/indexer.rs
@@ -26,7 +26,7 @@ impl Indexer {
     pub fn new() -> Self {
         let motor = Talon::new(robotmap::indexer::MOTOR, None);
         let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN);
-        let debouncer = Debouncer::new(Duration::from_secs_f64(INDEXER_LASER_DEBOUNCE_TIME_SECONDS), DebounceType::RISING);
+        let debouncer = Debouncer::new(Duration::from_secs_f64(INDEXER_LASER_DEBOUNCE_TIME_SECONDS), DebounceType::FALLING);
 
         Self { motor, laser_can, debouncer }
     }

From 2c43b10e73fc91309fe5f9a79f9e8626d7865836 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Thu, 27 Mar 2025 16:34:42 -0500
Subject: [PATCH 07/14] climber and auto

---
 src/auto/mod.rs           | 54 ++++++++++++++++++++++-----------------
 src/constants.rs          |  4 +--
 src/subsystems/climber.rs |  4 +--
 3 files changed, 34 insertions(+), 28 deletions(-)

diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 1aad7ee..b513bc9 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -240,13 +240,6 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
     ));
 
     join!(drive("Blue2", &mut drivetrain, 1), async {
-        elevator.set_target(ElevatorPosition::L2);
-        elevator.run_to_target_trapezoid();
-
-        indexer.set_speed(INTAKE_SPEED);
-        wait(|| indexer.is_laser_tripped()).await;
-        indexer.stop();
-
         elevator.set_target(ElevatorPosition::L4);
         elevator.run_to_target_trapezoid();
     });
@@ -425,13 +418,6 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
     drive("TushPush1", &mut drivetrain, 1).await?;
 
     join!(drive("TushPush1", &mut drivetrain, 2), async {
-        elevator.set_target(ElevatorPosition::L2);
-        elevator.run_to_target_trapezoid();
-
-        indexer.set_speed(INTAKE_SPEED);
-        wait(|| indexer.is_laser_tripped()).await;
-        indexer.stop();
-
         elevator.set_target(ElevatorPosition::L4);
         elevator.run_to_target_trapezoid();
     });
@@ -445,16 +431,36 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
         }
     }).await;
 
-    async_score(
-        &mut drivetrain,
-        LineupSide::Right,
-        &mut elevator,
-        &mut indexer,
-        ElevatorPosition::L4,
-        robot.dt,
-        if alliance_station().red() {Some(10)} else { Some(21)}
-    )
-        .await;
+    elevator.set_target(ElevatorPosition::L4);
+
+    join!(
+         timeout(Duration::from_secs_f64(2.), async {
+            loop {
+                drivetrain.update_limelight().await;
+                drivetrain.post_odo().await;
+
+                if drivetrain.lineup(LineupSide::Right,
+                    ElevatorPosition::L4,
+                    robot.dt,
+                    if alliance_station().red() {Some(10)} else { Some(21)}).await
+                {
+                    break;
+                }
+
+                sleep(Duration::from_millis(20)).await;
+            }
+        }),
+        elevator.run_to_target_trapezoid_async()
+    );
+
+    drivetrain.stop();
+    
+    indexer.set_speed(-0.4);
+
+    wait(|| !indexer.is_laser_tripped()).await;
+
+    sleep(Duration::from_secs_f64(1.5)).await;
+    indexer.stop();
 
     Ok(())
 }
\ No newline at end of file
diff --git a/src/constants.rs b/src/constants.rs
index dd5a617..266f01d 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -55,7 +55,7 @@ pub const HALF_FIELD_LENGTH_METERS: f64 = 8.05 / 2.;
 pub mod vision {
     use nalgebra::Vector2;
 
-    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -35.5; //Last measured 3/27 @ 9:27: -36.8, -37.3, -37.0
+    pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -35.2; //Last measured 3/27 @ 9:27: -36.8, -37.3, -37.0
     pub const LIMELIGHT_UPPER_YAW_DEGREES: f64 = 90.; // Counterclockwise positive
     pub const LIMELIGHT_UPPER_HEIGHT_INCHES: f64 = 20.92;
     pub const ROBOT_CENTER_TO_LIMELIGHT_UPPER_INCHES: Vector2<f64> = Vector2::new(11.118, 10.352);
@@ -130,7 +130,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 = 2;
+    pub const LASER_TRIP_DISTANCE_MM: i32 = 10;
     pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08;
     pub const INTAKE_SPEED: f64 = -0.25;
     pub const BOTTOM_SPEED: f64 = -0.35;
diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs
index 499f7f1..9ec1cb2 100644
--- a/src/subsystems/climber.rs
+++ b/src/subsystems/climber.rs
@@ -27,9 +27,9 @@ impl Climber {
     pub fn set(&self, speed: f64){
         self.motor.set(ControlMode::Percent, speed);
     }
-    
+
     pub fn climb(&self) {
-        if self.motor.get_position() < 330. {
+        if self.motor.get_position() < 380. {
             self.set(CLIMB_SPEED);
         } else {
             self.set(0.);

From 5d65c5c6447115f4f9a50c83238be7d605a68a5c Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Thu, 27 Mar 2025 20:16:20 -0500
Subject: [PATCH 08/14] constants changes

---
 src/constants.rs             | 2 +-
 src/lib.rs                   | 9 +++++----
 src/main.rs                  | 8 ++++----
 src/subsystems/drivetrain.rs | 1 +
 4 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/constants.rs b/src/constants.rs
index 266f01d..53ca1c8 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -135,7 +135,7 @@ pub mod indexer {
     pub const INTAKE_SPEED: f64 = -0.25;
     pub const BOTTOM_SPEED: f64 = -0.35;
     pub const L2_SPEED: f64 = -0.4;
-    pub const L3_SPEED: f64 = -0.4;
+    pub const L3_SPEED: f64 = -0.5;
     pub const L4_SPEED: f64 = -0.4;
 }
 pub mod climber {
diff --git a/src/lib.rs b/src/lib.rs
index f40249e..fff3608 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -7,10 +7,7 @@ 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, DebounceType, Debouncer, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide, Vision};
 use constants::joystick_map::*;
 use frcrs::ctre::ControlMode;
 use frcrs::input::Joystick;
@@ -63,6 +60,8 @@ pub struct Ferris {
     pub climb_handle: Option<AbortHandle>,
 
     pub dt: Duration,
+    
+    pub debouncer: Debouncer,
 }
 
 impl Default for Ferris {
@@ -93,6 +92,8 @@ impl Ferris {
             climb_handle: None,
 
             dt: Duration::from_millis(0),
+            
+            debouncer: Debouncer::new(Duration::from_secs_f64(0.25), DebounceType::RISING),
         }
     }
 
diff --git a/src/main.rs b/src/main.rs
index 4265339..aa3ac0a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -161,8 +161,8 @@ async fn teleop(robot: &mut Ferris) {
             if let Ok(mut indexer) = robot.indexer.try_borrow_mut() {
                 drivetrain.update_limelight().await;
                 drivetrain.post_odo().await;
-
-                let drivetrain_aligned = if robot.controllers.right_drive.get(LINEUP_LEFT) {
+                
+                let drivetrain_aligned = robot.debouncer.calculate(if robot.controllers.right_drive.get(LINEUP_LEFT) {
                     drivetrain
                         .lineup(LineupSide::Left, elevator.get_target(), robot.dt, None)
                         .await
@@ -182,12 +182,12 @@ async fn teleop(robot: &mut Ferris) {
                         .await;
 
                     false
-                };
+                });
 
                 if robot.controllers.left_drive.get_pov() != -1 {
                     elevator.set_target(ElevatorPosition::L3Algae);
                     elevator.run_to_target_trapezoid();
-                    
+
                     indexer.set_speed(L3_SPEED);
                 } else if robot.controllers.left_drive.get(SCORE_L2) {
                     score(
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 2cb3a8a..0d53712 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -64,6 +64,7 @@ pub enum DebounceType {
 
 /// https://docs.wpilib.org/en/stable/docs/software/advanced-controls/filters/debouncer.html
 /// but in rust
+#[derive(Clone)]
 pub struct Debouncer {
     bounce_time: Duration,
     debounce_type: DebounceType,

From 5ce8785ca0d25d7e71a1bd4b7e1d0275b5bfe2d7 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Thu, 27 Mar 2025 20:25:40 -0500
Subject: [PATCH 09/14] untested right 2 auto

---
 auto/Right2.traj | 259 +++++++++++++++++++++++++++++++++++++++++++++++
 src/auto/mod.rs  |  80 +++++++++++++++
 2 files changed, 339 insertions(+)
 create mode 100644 auto/Right2.traj

diff --git a/auto/Right2.traj b/auto/Right2.traj
new file mode 100644
index 0000000..4803276
--- /dev/null
+++ b/auto/Right2.traj
@@ -0,0 +1,259 @@
+{
+ "name":"Right2",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.18402099609375, "y":2.696911096572876, "heading":0.0, "intervals":43, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.393235683441162, "y":2.1473214626312256, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.4657111167907715, "y":2.810650110244751, "heading":-0.5209093488872486, "intervals":16, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.7834198474884033, "y":2.9654312133789062, "heading":-0.530215876376111, "intervals":58, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.1358485221862793, "y":0.9369845390319824, "heading":-2.2073044889150486, "intervals":55, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.4575648307800293, "y":2.81065034866333, "heading":-0.5209093488872486, "intervals":40, "split":true, "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":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":2.5}}, "enabled":true},
+    {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"7.18402099609375 m", "val":7.18402099609375}, "y":{"exp":"2.696911096572876 m", "val":2.696911096572876}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":43, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.393235683441162 m", "val":4.393235683441162}, "y":{"exp":"2.1473214626312256 m", "val":2.1473214626312256}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.4657111167907715 m", "val":3.4657111167907715}, "y":{"exp":"2.810650110244751 m", "val":2.810650110244751}, "heading":{"exp":"-0.5209093488872486 rad", "val":-0.5209093488872486}, "intervals":16, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.7834198474884033 m", "val":3.7834198474884033}, "y":{"exp":"2.9654312133789062 m", "val":2.9654312133789062}, "heading":{"exp":"-0.530215876376111 rad", "val":-0.530215876376111}, "intervals":58, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.1358485221862793 m", "val":1.1358485221862793}, "y":{"exp":"0.9369845390319824 m", "val":0.9369845390319824}, "heading":{"exp":"-2.2073044889150486 rad", "val":-2.2073044889150486}, "intervals":55, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.4575648307800293 m", "val":3.4575648307800293}, "y":{"exp":"2.81065034866333 m", "val":2.81065034866333}, "heading":{"exp":"-0.5209093488872486 rad", "val":-0.5209093488872486}, "intervals":40, "split":true, "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":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2.5 m / s ^ 2", "val":2.5}}}, "enabled":true},
+    {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.68545,2.67781,3.42667,5.73552,7.92127],
+  "samples":[
+    {"t":0.0, "x":7.18402, "y":2.69691, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.215, "ay":-1.15641, "alpha":0.32312, "fx":[-38.5691,-38.4872,-36.77558,-36.87433], "fy":[-18.74205,-20.43973,-20.66681,-18.83207]},
+    {"t":0.0392, "x":7.18232, "y":2.69602, "heading":0.0, "vx":-0.08682, "vy":-0.04533, "omega":0.01267, "ax":-2.22071, "ay":-1.14678, "alpha":0.28262, "fx":[-38.55281,-38.47832,-36.98817,-37.07511], "fy":[-18.69063,-20.19151,-20.37182,-18.77132]},
+    {"t":0.07839, "x":7.17721, "y":2.69337, "heading":0.0005, "vx":-0.17386, "vy":-0.09028, "omega":0.02374, "ax":-2.22618, "ay":-1.13606, "alpha":0.25844, "fx":[-38.58256,-38.4929,-37.20669,-37.18469], "fy":[-18.51595,-20.003,-20.07795,-18.69907]},
+    {"t":0.11759, "x":7.16869, "y":2.68895, "heading":0.00143, "vx":-0.26112, "vy":-0.13481, "omega":0.03387, "ax":-2.23203, "ay":-1.12446, "alpha":0.21191, "fx":[-38.55255,-38.50069,-37.3731,-37.4386], "fy":[-18.52036,-19.62471,-19.78324,-18.57832]},
+    {"t":0.15679, "x":7.15674, "y":2.68281, "heading":0.00275, "vx":-0.34861, "vy":-0.17888, "omega":0.04218, "ax":-2.2383, "ay":-1.11186, "alpha":0.19526, "fx":[-38.61702,-38.54559,-37.59089,-37.53788], "fy":[-18.28523,-19.42686,-19.47793,-18.4596]},
+    {"t":0.19598, "x":7.14135, "y":2.67494, "heading":0.00441, "vx":-0.43634, "vy":-0.22246, "omega":0.04983, "ax":-2.24503, "ay":-1.09814, "alpha":0.15371, "fx":[-38.61528,-38.58028,-37.75248,-37.8011], "fy":[-18.24487,-19.02596,-19.16172,-18.2834]},
+    {"t":0.23518, "x":7.12252, "y":2.66538, "heading":0.00636, "vx":-0.52434, "vy":-0.2655, "omega":0.05586, "ax":-2.25227, "ay":-1.08313, "alpha":0.1429, "fx":[-38.71388,-38.65771,-37.97827,-37.89178], "fy":[-17.94913,-18.79994,-18.83185,-18.11406]},
+    {"t":0.27438, "x":7.10024, "y":2.65414, "heading":0.00855, "vx":-0.61262, "vy":-0.30796, "omega":0.06146, "ax":-2.26007, "ay":-1.06666, "alpha":0.10462, "fx":[-38.73909,-38.71874,-38.14023,-38.17469], "fy":[-17.85588,-18.35523,-18.48414,-17.87883]},
+    {"t":0.31357, "x":7.07449, "y":2.64125, "heading":0.01096, "vx":-0.70121, "vy":-0.34977, "omega":0.06556, "ax":-2.26851, "ay":-1.04849, "alpha":0.09894, "fx":[-38.87323,-38.83058,-38.3827,-38.26035], "fy":[-17.48983,-18.09264,-18.10983,-17.6457]},
+    {"t":0.35277, "x":7.04527, "y":2.62673, "heading":0.01353, "vx":-0.79013, "vy":-0.39087, "omega":0.06944, "ax":-2.27766, "ay":-1.02836, "alpha":0.06304, "fx":[-38.92634,-38.91892,-38.551,-38.57279], "fy":[-17.328,-17.58805,-17.71308,-17.33925]},
+    {"t":0.39196, "x":7.01255, "y":2.61062, "heading":0.01625, "vx":-0.8794, "vy":-0.43117, "omega":0.07191, "ax":-2.28759, "ay":-1.00594, "alpha":0.06233, "fx":[-39.09876,-39.06702,-38.81662,-38.66259], "fy":[-16.86242,-17.27161,-17.27828,-17.03045]},
+    {"t":0.43116, "x":6.97632, "y":2.59295, "heading":0.01907, "vx":-0.96907, "vy":-0.4706, "omega":0.07435, "ax":-2.29841, "ay":-0.98081, "alpha":0.02741, "fx":[-39.18323,-39.18742,-39.00025,-39.01028], "fy":[-16.62401,-16.68063,-16.80158,-16.62726]},
+    {"t":0.47036, "x":6.93657, "y":2.57375, "heading":0.02198, "vx":-1.05916, "vy":-0.50905, "omega":0.07543, "ax":-2.31022, "ay":-0.95249, "alpha":0.0302, "fx":[-39.4004,-39.3811,-39.30173,-39.10174], "fy":[-16.05103,-16.27108,-16.27108,-16.21285]},
+    {"t":0.50955, "x":6.89328, "y":2.55307, "heading":0.02494, "vx":-1.14971, "vy":-0.54638, "omega":0.07661, "ax":-2.32315, "ay":-0.92031, "alpha":-0.00384, "fx":[-39.52001,-39.5344,-39.50553,-39.50446], "fy":[-15.68576,-15.56324,-15.68355,-15.68449]},
+    {"t":0.54875, "x":6.84643, "y":2.53094, "heading":0.02794, "vx":-1.24077, "vy":-0.58245, "omega":0.07646, "ax":-2.33732, "ay":-0.88348, "alpha":0.00269, "fx":[-39.7898,-39.78125,-39.85301,-39.60423], "fy":[-14.953,-15.01813,-15.01506,-15.12481]},
+    {"t":0.58795, "x":6.796, "y":2.50743, "heading":0.03094, "vx":-1.33238, "vy":-0.61708, "omega":0.07657, "ax":-2.35286, "ay":-0.84094, "alpha":-0.0325, "fx":[-39.94994,-39.97489,-40.08619,-40.07469], "fy":[-14.4195,-14.12128,-14.25842,-14.41707]},
+    {"t":0.62714, "x":6.74197, "y":2.4826, "heading":0.03394, "vx":-1.42461, "vy":-0.65005, "omega":0.07529, "ax":-2.3699, "ay":-0.79129, "alpha":-0.0219, "fx":[-40.28284,-40.28543,-40.49403,-40.18287], "fy":[-13.45724,-13.3746,-13.37206,-13.63481]},
+    {"t":0.66634, "x":6.68431, "y":2.45651, "heading":0.03689, "vx":-1.5175, "vy":-0.68106, "omega":0.07443, "ax":-2.38853, "ay":-0.73271, "alpha":-0.05882, "fx":[-40.4878,-40.5213,-40.7623,-40.74131], "fy":[-12.65368,-12.1968,-12.34864,-12.65361]},
+    {"t":0.70554, "x":6.62299, "y":2.42925, "heading":0.03981, "vx":-1.61112, "vy":-0.70978, "omega":0.07213, "ax":-2.40874, "ay":-0.66268, "alpha":-0.04393, "fx":[-40.88439,-40.89632,-41.23321,-40.87419], "fy":[-11.313,-11.12247,-11.12447,-11.52786]},
+    {"t":0.74473, "x":6.55799, "y":2.40092, "heading":0.04264, "vx":-1.70554, "vy":-0.73576, "omega":0.07041, "ax":-2.43033, "ay":-0.57773, "alpha":-0.08396, "fx":[-41.13428,-41.17261,-41.53946,-41.51077], "fy":[-10.08656,-9.48165,-9.64693,-10.09308]},
+    {"t":0.78393, "x":6.48927, "y":2.37164, "heading":0.0454, "vx":-1.8008, "vy":-0.7584, "omega":0.06712, "ax":-2.45265, "ay":-0.473, "alpha":-0.06654, "fx":[-41.5814,-41.60516,-42.06609,-41.62296], "fy":[-8.17372,-7.81524,-7.82717,-8.36615]},
+    {"t":0.82313, "x":6.41681, "y":2.34155, "heading":0.04803, "vx":-1.89693, "vy":-0.77694, "omega":0.06451, "ax":-2.47413, "ay":-0.34151, "alpha":-0.10778, "fx":[-41.81643,-41.85314,-42.3499,-42.31721], "fy":[-6.12645,-5.40764,-5.5562,-6.14571]},
+    {"t":0.86232, "x":6.34055, "y":2.31084, "heading":0.05056, "vx":-1.99391, "vy":-0.79033, "omega":0.06028, "ax":-2.49125, "ay":-0.17323, "alpha":-0.09088, "fx":[-42.18513,-42.21681,-42.81108,-42.28866], "fy":[-3.17901,-2.62639,-2.65708,-3.32382]},
+    {"t":0.90152, "x":6.26048, "y":2.27972, "heading":0.05292, "vx":-2.09156, "vy":-0.79712, "omega":0.05672, "ax":-2.49644, "ay":0.04616, "alpha":-0.13034, "fx":[-42.13062,-42.15854,-42.79741,-42.76835], "fy":[0.42681,1.20298,1.12582,0.38491]},
+    {"t":0.94072, "x":6.17658, "y":2.24852, "heading":0.05514, "vx":-2.18941, "vy":-0.79531, "omega":0.05161, "ax":-2.47368, "ay":0.33587, "alpha":-0.12519, "fx":[-41.81164,-41.83709,-42.58438,-42.07333], "fy":[5.29613,6.15656,6.09484,5.30438]},
+    {"t":0.97991, "x":6.08887, "y":2.2176, "heading":0.05717, "vx":-2.28637, "vy":-0.78214, "omega":0.0467, "ax":-2.39066, "ay":0.71676, "alpha":-0.1517, "fx":[-40.25035,-40.2706,-41.07436,-41.06269], "fy":[11.82082,12.55782,12.64895,11.74026]},
+    {"t":1.01911, "x":5.99741, "y":2.18749, "heading":0.059, "vx":-2.38007, "vy":-0.75405, "omega":0.04076, "ax":-2.19045, "ay":1.19496, "alpha":-0.17068, "fx":[-36.89112,-36.87774,-37.83353,-37.43333], "fy":[19.69177,20.91915,20.81038,19.88213]},
+    {"t":1.05831, "x":5.90244, "y":2.15886, "heading":0.06059, "vx":-2.46593, "vy":-0.70721, "omega":0.03407, "ax":-1.80498, "ay":1.72212, "alpha":-0.18101, "fx":[-30.15402,-30.19883,-31.22523,-31.23068], "fy":[28.90538,29.61348,29.87055,28.78181]},
+    {"t":1.0975, "x":5.8044, "y":2.13246, "heading":0.06193, "vx":-2.53668, "vy":-0.63971, "omega":0.02697, "ax":-1.22834, "ay":2.17126, "alpha":-0.22901, "fx":[-20.3293,-20.31115,-21.57755,-21.35683], "fy":[36.18912,37.65344,37.52342,36.36404]},
+    {"t":1.1367, "x":5.70402, "y":2.10905, "heading":0.06299, "vx":-2.58483, "vy":-0.5546, "omega":0.018, "ax":-0.58053, "ay":2.42649, "alpha":-0.24276, "fx":[-9.11639,-9.19673,-10.6172,-10.5683], "fy":[40.74412,41.76417,41.95773,40.62973]},
+    {"t":1.17589, "x":5.60226, "y":2.08918, "heading":0.06369, "vx":-2.60758, "vy":-0.45949, "omega":0.00848, "ax":-0.009, "ay":2.49553, "alpha":-0.3068, "fx":[0.73926,0.62374,-1.04785,-0.92766], "fy":[41.56918,43.30739,43.21455,41.70204]},
+    {"t":1.21509, "x":5.50005, "y":2.07308, "heading":0.06402, "vx":-2.60794, "vy":-0.36168, "omega":-0.00354, "ax":0.42903, "ay":2.459, "alpha":-0.32772, "fx":[8.33179,8.19073,6.25564,6.41236], "fy":[41.06661,42.5105,42.73762,40.99312]},
+    {"t":1.25429, "x":5.39815, "y":2.0608, "heading":0.06388, "vx":-2.59112, "vy":-0.26529, "omega":-0.01639, "ax":0.74749, "ay":2.38216, "alpha":-0.41431, "fx":[13.98809,13.7329,11.52776,11.6093], "fy":[39.32742,41.62479,41.57527,39.55165]},
+    {"t":1.29348, "x":5.29717, "y":2.05223, "heading":0.06324, "vx":-2.56182, "vy":-0.17192, "omega":-0.03263, "ax":0.97818, "ay":2.29755, "alpha":-0.44539, "fx":[18.04223,17.80169,15.22041,15.48964], "fy":[37.99389,40.05511,40.32215,37.95149]},
+    {"t":1.33268, "x":5.1975, "y":2.04725, "heading":0.06196, "vx":-2.52348, "vy":-0.08187, "omega":-0.05009, "ax":1.14818, "ay":2.21788, "alpha":-0.55413, "fx":[21.2512,20.87205,17.92562,18.07207], "fy":[36.16336,39.17755,39.16246,36.39863]},
+    {"t":1.37188, "x":5.09947, "y":2.04575, "heading":0.06, "vx":-2.47847, "vy":0.00507, "omega":-0.07181, "ax":1.27646, "ay":2.14694, "alpha":-0.61035, "fx":[23.6154,23.24399,19.79975,20.18996], "fy":[34.96831,37.9389,38.2144,34.95397]},
+    {"t":1.41107, "x":5.00331, "y":2.0476, "heading":0.05719, "vx":-2.42844, "vy":0.08922, "omega":-0.09573, "ax":1.37562, "ay":2.08507, "alpha":-0.74286, "fx":[25.69124,25.17926,21.2247,21.50074], "fy":[33.38567,37.40209,37.43359,33.64476]},
+    {"t":1.45027, "x":4.90918, "y":2.0527, "heading":0.05343, "vx":-2.37452, "vy":0.17095, "omega":-0.12485, "ax":1.45401, "ay":2.03143, "alpha":-0.83239, "fx":[27.295,26.77099,22.17249,22.69046], "fy":[32.37483,36.57972,36.85752,32.40401]},
+    {"t":1.48947, "x":4.81722, "y":2.06096, "heading":0.04854, "vx":-2.31753, "vy":0.25057, "omega":-0.15747, "ax":1.51721, "ay":1.98487, "alpha":-0.99412, "fx":[28.84637,28.19485,22.89478,23.2932], "fy":[30.99948,36.32544,36.44308,31.28035]},
+    {"t":1.52866, "x":4.72755, "y":2.0723, "heading":0.04237, "vx":-2.25806, "vy":0.32837, "omega":-0.19644, "ax":1.56907, "ay":1.94431, "alpha":-1.12407, "fx":[30.10544,29.42554,23.29464,23.93228], "fy":[30.05973,35.84462,36.21006,30.17397]},
+    {"t":1.56786, "x":4.64024, "y":2.08667, "heading":0.03467, "vx":-2.19656, "vy":0.40458, "omega":-0.2405, "ax":1.61229, "ay":1.90878, "alpha":-1.32569, "fx":[31.42498,30.63984,23.56856,24.0648], "fy":[28.7762,35.83411,36.11753,29.14306]},
+    {"t":1.60706, "x":4.55539, "y":2.10399, "heading":0.02524, "vx":-2.13336, "vy":0.4794, "omega":-0.29246, "ax":1.64878, "ay":1.87748, "alpha":-1.50597, "fx":[32.54673,31.73192,23.59719,24.30537], "fy":[27.80089,35.6476,36.20162,28.09119]},
+    {"t":1.64625, "x":4.47303, "y":2.12423, "heading":0.01378, "vx":-2.06874, "vy":0.55299, "omega":-0.35149, "ax":1.67997, "ay":1.84974, "alpha":-1.75986, "fx":[33.79242,32.91907,23.54325,24.04821], "fy":[26.50665,35.8284,36.43535,27.08395]},
+    {"t":1.68545, "x":4.39324, "y":2.14732, "heading":0.0, "vx":-2.00289, "vy":0.62549, "omega":-0.42047, "ax":1.74203, "ay":1.78857, "alpha":-1.17564, "fx":[33.03563,32.62038,26.24215,26.62733], "fy":[27.13015,33.18147,33.8946,27.48606]},
+    {"t":1.71464, "x":4.33552, "y":2.16634, "heading":-0.01227, "vx":-1.95204, "vy":0.6777, "omega":-0.45479, "ax":1.84429, "ay":1.68525, "alpha":-1.08125, "fx":[34.47086,34.11341,28.43575,28.46314], "fy":[25.48276,31.30057,31.7943,26.08508]},
+    {"t":1.74382, "x":4.27933, "y":2.18684, "heading":-0.02555, "vx":-1.89821, "vy":0.72689, "omega":-0.48634, "ax":1.94674, "ay":1.56568, "alpha":-0.94884, "fx":[35.76431,35.54575,30.47224,30.67135], "fy":[23.86079,28.84741,29.51535,24.30359]},
+    {"t":1.77301, "x":4.22476, "y":2.20872, "heading":-0.03974, "vx":-1.84139, "vy":0.77258, "omega":-0.51404, "ax":2.04846, "ay":1.4299, "alpha":-0.88111, "fx":[37.26713,37.12671,32.58192,32.39942], "fy":[21.62576,26.43611,26.94614,22.28054]},
+    {"t":1.8022, "x":4.17188, "y":2.23188, "heading":-0.05474, "vx":-1.7816, "vy":0.81432, "omega":-0.53975, "ax":2.14688, "ay":1.27728, "alpha":-0.76319, "fx":[38.54475,38.51506,34.49483,34.51636], "fy":[19.42025,23.48576,24.12054,19.87791]},
+    {"t":1.83138, "x":4.1208, "y":2.25619, "heading":-0.0705, "vx":-1.71894, "vy":0.8516, "omega":-0.56203, "ax":2.23891, "ay":1.10795, "alpha":-0.7067, "fx":[39.93423,39.97603,36.40703,36.01529], "fy":[16.6423,20.50441,20.99656,17.24034]},
+    {"t":1.86057, "x":4.07158, "y":2.28152, "heading":-0.0869, "vx":-1.6536, "vy":0.88394, "omega":-0.58266, "ax":2.32122, "ay":0.92304, "alpha":-0.60142, "fx":[40.98137,41.0948,37.98805,37.86865], "fy":[13.82441,17.08663,17.63397,14.25725]},
+    {"t":1.88976, "x":4.02431, "y":2.30771, "heading":-0.10391, "vx":-1.58585, "vy":0.91088, "omega":-0.60021, "ax":2.39052, "ay":0.72479, "alpha":-0.54972, "fx":[42.01741,42.1872,39.47703,38.96635], "fy":[10.5853,13.59715,14.02868,11.10261]},
+    {"t":1.91895, "x":3.97904, "y":2.3346, "heading":-0.12143, "vx":-1.51607, "vy":0.93203, "omega":-0.61626, "ax":2.44396, "ay":0.51659, "alpha":-0.4531, "fx":[42.61637,42.82214,40.52878,40.31698], "fy":[7.33542,9.84565,10.25962,7.70721]},
+    {"t":1.94813, "x":3.93583, "y":2.36203, "heading":-0.13941, "vx":-1.44474, "vy":0.94711, "omega":-0.62948, "ax":2.47955, "ay":0.3027, "alpha":-0.3997, "fx":[43.10683,43.34171,41.41698,40.84022], "fy":[3.90564,6.04273,6.38721,4.26012]},
+    {"t":1.97732, "x":3.89472, "y":2.3898, "heading":-0.15779, "vx":-1.37237, "vy":0.95594, "omega":-0.64115, "ax":2.49643, "ay":0.08791, "alpha":-0.31063, "fx":[43.11885,43.35146,41.80995,41.57365], "fy":[0.47685,2.25064,2.49483,0.7592]},
+    {"t":2.00651, "x":3.85573, "y":2.41774, "heading":-0.1765, "vx":-1.29951, "vy":0.95851, "omega":-0.65021, "ax":2.49497, "ay":-0.12305, "alpha":-0.25187, "fx":[43.00654,43.22103,42.04462,41.48261], "fy":[-2.82296,-1.56446,-1.32989,-2.65499]},
+    {"t":2.03569, "x":3.81886, "y":2.44566, "heading":-0.19548, "vx":-1.22669, "vy":0.95492, "omega":-0.65756, "ax":2.47668, "ay":-0.32598, "alpha":-0.1661, "fx":[42.44366,42.62055,41.81016,41.63624], "fy":[-6.10347,-5.09117,-5.04468,-5.93971]},
+    {"t":2.06488, "x":3.78411, "y":2.4734, "heading":-0.21467, "vx":-1.1544, "vy":0.9454, "omega":-0.66241, "ax":2.4439, "ay":-0.51753, "alpha":-0.0987, "fx":[41.81862,41.91303,41.49506,41.05319], "fy":[-9.00191,-8.63481,-8.54359,-9.03211]},
+    {"t":2.09407, "x":3.75146, "y":2.50077, "heading":-0.234, "vx":-1.08307, "vy":0.9303, "omega":-0.66529, "ax":2.3994, "ay":-0.69546, "alpha":-0.01344, "fx":[40.82825,40.85116,40.79169,40.7811], "fy":[-11.88763,-11.69085,-11.85796,-11.88163]},
+    {"t":2.12326, "x":3.72087, "y":2.52762, "heading":-0.25342, "vx":-1.01304, "vy":0.91, "omega":-0.66568, "ax":2.34607, "ay":-0.85852, "alpha":0.06535, "fx":[39.86481,39.73198,40.11111,39.91589], "fy":[-14.24027,-14.79477,-14.88486,-14.49251]},
+    {"t":2.15244, "x":3.6923, "y":2.55382, "heading":-0.27285, "vx":-0.94456, "vy":0.88494, "omega":-0.66378, "ax":2.28662, "ay":-1.00638, "alpha":0.1536, "fx":[38.63795,38.39439,39.14177,39.40484], "fy":[-16.61456,-17.32102,-17.71572,-16.82139]},
+    {"t":2.18163, "x":3.6657, "y":2.57922, "heading":-0.29222, "vx":-0.87782, "vy":0.85557, "omega":-0.65929, "ax":2.22341, "ay":-1.13939, "alpha":0.24487, "fx":[37.5123,37.03892,38.27889,38.44828], "fy":[-18.40061,-19.93113,-20.26186,-18.92895]},
+    {"t":2.21082, "x":3.64103, "y":2.60371, "heading":-0.31147, "vx":-0.81293, "vy":0.82231, "omega":-0.65215, "ax":2.15839, "ay":-1.25836, "alpha":0.33851, "fx":[36.20636,35.57515,37.20864,37.86405], "fy":[-20.26924,-21.94797,-22.63598,-20.76404]},
+    {"t":2.24, "x":3.61822, "y":2.62717, "heading":-0.3305, "vx":-0.74993, "vy":0.78559, "omega":-0.64227, "ax":2.09306, "ay":-1.36438, "alpha":0.44175, "fx":[35.04762,34.11487,36.30025,36.94687], "fy":[-21.55984,-24.1047,-24.75439,-22.41159]},
+    {"t":2.26919, "x":3.59723, "y":2.64952, "heading":-0.34925, "vx":-0.68884, "vy":0.74576, "omega":-0.62937, "ax":2.02857, "ay":-1.45865, "alpha":0.54606, "fx":[33.76565,32.61452,35.23265,36.40874], "fy":[-22.95641,-25.70129,-26.7553,-23.83186]},
+    {"t":2.29838, "x":3.57798, "y":2.67066, "heading":-0.36762, "vx":-0.62963, "vy":0.70319, "omega":-0.61344, "ax":1.96571, "ay":-1.54241, "alpha":0.6658, "fx":[32.6475,31.11891,34.34983,35.62853], "fy":[-23.80656,-27.48082,-28.54921,-25.10746]},
+    {"t":2.32757, "x":3.56045, "y":2.69053, "heading":-0.38552, "vx":-0.57226, "vy":0.65817, "omega":-0.594, "ax":1.90503, "ay":-1.61685, "alpha":0.78117, "fx":[31.44897,29.63176,33.34847,35.18683], "fy":[-24.81563,-28.73436,-30.26277,-26.19614]},
+    {"t":2.35675, "x":3.54455, "y":2.70905, "heading":-0.40286, "vx":-0.51666, "vy":0.61098, "omega":-0.5712, "ax":1.84686, "ay":-1.68307, "alpha":0.91687, "fx":[30.40856,28.13832,32.53431,34.5771], "fy":[-25.32164,-30.21126,-31.81118,-27.17034]},
+    {"t":2.38594, "x":3.53026, "y":2.72617, "heading":-0.41953, "vx":-0.46275, "vy":0.56186, "omega":-0.54444, "ax":1.79139, "ay":-1.74207, "alpha":1.04866, "fx":[29.31448,26.67187,31.62045,34.27722], "fy":[-25.98982,-31.20581,-33.3273,-28.00579]},
+    {"t":2.41513, "x":3.51752, "y":2.74183, "heading":-0.43542, "vx":-0.41047, "vy":0.51101, "omega":-0.51383, "ax":1.73869, "ay":-1.79475, "alpha":1.20364, "fx":[28.36406,25.18625,30.889,33.85889], "fy":[-26.20104,-32.45267,-34.71094,-28.74779]},
+    {"t":2.44431, "x":3.50628, "y":2.75598, "heading":-0.45042, "vx":-0.35972, "vy":0.45863, "omega":-0.4787, "ax":1.68875, "ay":-1.84188, "alpha":1.35504, "fx":[27.37737,23.7311,30.07391,33.718], "fy":[-26.58049,-33.26778,-36.08927,-29.38162]},
+    {"t":2.4735, "x":3.4965, "y":2.76858, "heading":-0.46439, "vx":-0.31043, "vy":0.40487, "omega":-0.43915, "ax":1.64151, "ay":-1.88415, "alpha":1.53103, "fx":[26.51401,22.24765,29.42807,33.49674], "fy":[-26.55091,-34.32688,-37.37508,-29.9428]},
+    {"t":2.50269, "x":3.48814, "y":2.77959, "heading":-0.47721, "vx":-0.26252, "vy":0.34987, "omega":-0.39447, "ax":1.59688, "ay":-1.92218, "alpha":1.7074, "fx":[25.62369,20.78346,28.71247,33.53033], "fy":[-26.6693,-35.01004,-38.68577,-30.41765]},
+    {"t":2.53187, "x":3.48115, "y":2.78898, "heading":-0.48872, "vx":-0.21591, "vy":0.29377, "omega":-0.34463, "ax":1.55474, "ay":-1.95647, "alpha":1.90872, "fx":[24.83239,19.28204,28.15644,33.512], "fy":[-26.42401,-35.93913,-39.92345,-30.82923]},
+    {"t":2.56106, "x":3.47551, "y":2.79673, "heading":-0.49878, "vx":-0.17053, "vy":0.23667, "omega":-0.28892, "ax":1.51496, "ay":-1.98747, "alpha":2.11404, "fx":[24.0211,17.78406,27.54334,33.72789], "fy":[-26.30231,-36.53965,-41.20648,-31.17703]},
+    {"t":2.59025, "x":3.47118, "y":2.80279, "heading":-0.50721, "vx":-0.12632, "vy":0.17866, "omega":-0.22722, "ax":1.47741, "ay":-2.01559, "alpha":2.34588, "fx":[23.2786,16.23978,27.08256,33.92046], "fy":[-25.85342,-37.39334,-42.43644,-31.45506]},
+    {"t":2.61944, "x":3.46813, "y":2.80714, "heading":-0.51385, "vx":-0.08319, "vy":0.11983, "omega":-0.15875, "ax":1.44195, "ay":-2.04115, "alpha":2.58588, "fx":[22.51995,14.68126,26.57925,34.32822], "fy":[-25.5025,-37.95613,-43.7378,-31.68079]},
+    {"t":2.64862, "x":3.46631, "y":2.80977, "heading":-0.51848, "vx":-0.04111, "vy":0.06026, "omega":-0.08328, "ax":1.40845, "ay":-2.06444, "alpha":2.85323, "fx":[21.79532,13.06894,26.22883,34.73617], "fy":[-24.85727,-38.78172,-44.99325,-31.83]},
+    {"t":2.67781, "x":3.46571, "y":2.81065, "heading":-0.52091, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.10367, "ay":1.34866, "alpha":0.85629, "fx":[34.76789,32.81119,36.82168,38.73085], "fy":[26.26397,22.17975,19.66713,23.65019]},
+    {"t":2.72461, "x":3.46802, "y":2.81213, "heading":-0.52091, "vx":0.09846, "vy":0.06312, "omega":0.04008, "ax":2.10785, "ay":1.34302, "alpha":0.63915, "fx":[35.11973,33.58575,36.67879,38.03119], "fy":[25.2609,22.35858,20.3562,23.40199]},
+    {"t":2.77142, "x":3.47493, "y":2.81655, "heading":-0.51903, "vx":0.19711, "vy":0.12598, "omega":0.06999, "ax":2.11285, "ay":1.33491, "alpha":0.46158, "fx":[35.38374,34.35235,36.4969,37.52295], "fy":[24.51037,22.27328,20.94439,23.09792]},
+    {"t":2.81822, "x":3.48647, "y":2.82391, "heading":-0.51576, "vx":0.296, "vy":0.18846, "omega":0.0916, "ax":2.12001, "ay":1.32319, "alpha":0.26532, "fx":[35.77162,35.10598,36.44439,36.92124], "fy":[23.49024,22.3057,21.44466,22.78751]},
+    {"t":2.86502, "x":3.50265, "y":2.83418, "heading":-0.51147, "vx":0.39523, "vy":0.25039, "omega":0.10401, "ax":2.13111, "ay":1.30473, "alpha":0.10677, "fx":[36.11527,35.90729,36.37471,36.60079], "fy":[22.64127,22.00569,21.81051,22.31486]},
+    {"t":2.91183, "x":3.52348, "y":2.84733, "heading":-0.5066, "vx":0.49497, "vy":0.31146, "omega":0.10901, "ax":2.15058, "ay":1.27143, "alpha":-0.09259, "fx":[36.73711,36.8644,36.55363,36.16802], "fy":[21.22475,21.7235,21.9153,21.64306]},
+    {"t":2.95863, "x":3.549, "y":2.8633, "heading":-0.5015, "vx":0.59563, "vy":0.37096, "omega":0.10468, "ax":2.19354, "ay":1.19347, "alpha":-0.24139, "fx":[37.60477,38.17688,37.0068,36.4577], "fy":[19.42527,20.36648,21.28096,20.1299]},
+    {"t":3.00544, "x":3.57928, "y":2.88197, "heading":-0.4966, "vx":0.69829, "vy":0.42682, "omega":0.09338, "ax":2.35716, "ay":0.80879, "alpha":-0.44809, "fx":[40.63672,41.63536,39.62238,38.48408], "fy":[12.0339,14.20851,15.42722,13.3598]},
+    {"t":3.05224, "x":3.61455, "y":2.90283, "heading":-0.49223, "vx":0.80862, "vy":0.46468, "omega":0.07241, "ax":-1.61161, "ay":-1.89998, "alpha":-0.63548, "fx":[-26.58868,-25.15316,-28.2338,-29.67657], "fy":[-34.68801,-31.59534,-30.03764,-32.95134]},
+    {"t":3.09904, "x":3.65063, "y":2.9225, "heading":-0.48884, "vx":0.73319, "vy":0.37575, "omega":0.04266, "ax":-1.94028, "ay":-1.57192, "alpha":-0.78371, "fx":[-32.06367,-30.16338,-34.19029,-35.59697], "fy":[-29.59166,-26.05439,-23.70414,-27.60174]},
+    {"t":3.14585, "x":3.68282, "y":2.93836, "heading":-0.48684, "vx":0.64237, "vy":0.30218, "omega":0.00598, "ax":-1.99836, "ay":-1.49932, "alpha":-1.0376, "fx":[-32.62461,-30.38659,-35.3918,-37.56317], "fy":[-29.46019,-24.4138,-21.64358,-26.49427]},
+    {"t":3.19265, "x":3.7107, "y":2.95086, "heading":-0.48656, "vx":0.54884, "vy":0.232, "omega":-0.04258, "ax":-2.02232, "ay":-1.46768, "alpha":-1.25302, "fx":[-32.80734,-29.96063,-36.17292,-38.65527], "fy":[-29.63968,-23.85302,-20.18063,-26.18624]},
+    {"t":3.23945, "x":3.73417, "y":2.96012, "heading":-0.48856, "vx":0.45419, "vy":0.16331, "omega":-0.10123, "ax":-2.03538, "ay":-1.44999, "alpha":-1.53308, "fx":[-32.63246,-29.27244,-36.7031,-39.87664], "fy":[-30.49701,-23.21103,-18.87686,-26.07094]},
+    {"t":3.28626, "x":3.7532, "y":2.96617, "heading":-0.4933, "vx":0.35893, "vy":0.09545, "omega":-0.17298, "ax":-2.04359, "ay":-1.43869, "alpha":-1.80995, "fx":[-32.49135,-28.35927,-37.24915,-40.94384], "fy":[-31.28515,-22.91059,-17.56723,-26.12405]},
+    {"t":3.33306, "x":3.76776, "y":2.96906, "heading":-0.50139, "vx":0.26328, "vy":0.02811, "omega":-0.25769, "ax":-2.04924, "ay":-1.43085, "alpha":-2.14662, "fx":[-32.19808,-27.3007,-37.70897,-42.21989], "fy":[-32.51686,-22.48252,-16.16907,-26.18509]},
+    {"t":3.37986, "x":3.77784, "y":2.96881, "heading":-0.51345, "vx":0.16737, "vy":-0.03886, "omega":-0.35816, "ax":-2.05335, "ay":-1.42509, "alpha":-2.50176, "fx":[-31.97975,-26.00892,-38.1991,-43.51995], "fy":[-33.7225,-22.27857,-14.65313,-26.3075]},
+    {"t":3.42667, "x":3.78342, "y":2.96543, "heading":-0.53022, "vx":0.07127, "vy":-0.10556, "omega":-0.47525, "ax":-2.05451, "ay":-1.42218, "alpha":-1.88972, "fx":[-32.83826,-28.12274,-37.262,-41.56352], "fy":[-31.34576,-22.84876,-16.99585,-25.57287]},
+    {"t":3.46648, "x":3.78463, "y":2.9601, "heading":-0.54913, "vx":-0.01052, "vy":-0.16217, "omega":-0.55048, "ax":-2.05524, "ay":-1.42221, "alpha":-1.6216, "fx":[-33.20049,-29.18557,-36.82623,-40.62381], "fy":[-30.43532,-23.06665,-17.95816,-25.30524]},
+    {"t":3.50628, "x":3.78258, "y":2.95252, "heading":-0.57105, "vx":-0.09233, "vy":-0.21879, "omega":-0.61503, "ax":-2.05546, "ay":-1.42185, "alpha":-1.40365, "fx":[-33.56873,-29.87011,-36.47102,-39.94103], "fy":[-29.55215,-23.40455,-18.78799,-24.9963]},
+    {"t":3.54609, "x":3.77728, "y":2.94268, "heading":-0.59553, "vx":-0.17416, "vy":-0.27539, "omega":-0.67091, "ax":-2.05569, "ay":-1.42146, "alpha":-1.19256, "fx":[-33.85689,-30.67839,-36.13156,-39.19999], "fy":[-28.80985,-23.53902,-19.56994,-24.79586]},
+    {"t":3.5859, "x":3.76872, "y":2.9306, "heading":-0.62224, "vx":-0.25599, "vy":-0.33197, "omega":-0.71838, "ax":-2.05594, "ay":-1.42104, "alpha":-1.01521, "fx":[-34.15325,-31.22577,-35.87689,-38.62818], "fy":[-28.06592,-23.80921,-20.25228,-24.55881]},
+    {"t":3.62571, "x":3.7569, "y":2.91625, "heading":-0.65084, "vx":-0.33783, "vy":-0.38854, "omega":-0.75879, "ax":-2.05622, "ay":-1.42059, "alpha":-0.83885, "fx":[-34.35355,-31.92963,-35.62057,-37.99904], "fy":[-27.44968,-23.86244,-20.91343,-24.42979]},
+    {"t":3.66552, "x":3.74182, "y":2.89966, "heading":-0.68104, "vx":-0.41969, "vy":-0.44509, "omega":-0.79219, "ax":-2.05652, "ay":-1.4201, "alpha":-0.69475, "fx":[-34.57784,-32.36715,-35.45749,-37.52069], "fy":[-26.81662,-24.06857,-21.47517,-24.26131]},
+    {"t":3.70532, "x":3.72348, "y":2.88082, "heading":-0.71258, "vx":-0.50155, "vy":-0.50162, "omega":-0.81984, "ax":-2.05685, "ay":-1.41955, "alpha":-0.54596, "fx":[-34.69834,-32.99016,-35.27857,-36.97836], "fy":[-26.30493,-24.04356,-22.03459,-24.20181]},
+    {"t":3.74513, "x":3.70189, "y":2.85973, "heading":-0.74521, "vx":-0.58343, "vy":-0.55813, "omega":-0.84158, "ax":-2.0572, "ay":-1.41896, "alpha":-0.42944, "fx":[-34.86045,-33.3302,-35.19958,-36.57965], "fy":[-25.7619,-24.19408,-22.49467,-24.09383]},
+    {"t":3.78494, "x":3.67703, "y":2.83638, "heading":-0.77871, "vx":-0.66532, "vy":-0.61462, "omega":-0.85867, "ax":-2.0576, "ay":-1.41831, "alpha":-0.30251, "fx":[-34.90885,-33.89086,-35.08405,-36.11305], "fy":[-25.33975,-24.09629,-22.9695,-24.09438]},
+    {"t":3.82475, "x":3.64892, "y":2.81079, "heading":-0.8129, "vx":-0.74723, "vy":-0.67108, "omega":-0.87071, "ax":-2.05804, "ay":-1.41758, "alpha":-0.20961, "fx":[-35.01787,-34.14381,-35.07856,-35.78641], "fy":[-24.87482,-24.20259,-23.33628,-24.03681]},
+    {"t":3.86455, "x":3.61754, "y":2.78296, "heading":-0.84756, "vx":-0.82916, "vy":-0.72751, "omega":-0.87906, "ax":-2.05853, "ay":-1.41677, "alpha":-0.10072, "fx":[-35.00647,-34.65058,-35.01411,-35.38873], "fy":[-24.53101,-24.04587,-23.73392,-24.08462]},
+    {"t":3.90436, "x":3.5829, "y":2.75287, "heading":-0.88255, "vx":-0.9111, "vy":-0.78391, "omega":-0.88307, "ax":-2.05908, "ay":-1.41586, "alpha":-0.02897, "fx":[-35.07464,-34.81789,-35.07191,-35.13271], "fy":[-24.13648,-24.1169,-24.01456,-24.06568]},
+    {"t":3.94417, "x":3.545, "y":2.72054, "heading":-0.9177, "vx":-0.99307, "vy":-0.84027, "omega":-0.88422, "ax":-2.05969, "ay":-1.41484, "alpha":0.06509, "fx":[-35.01439,-35.28276,-35.04417,-34.79789], "fy":[-23.86695,-23.90419,-24.34239,-24.15025]},
+    {"t":3.98398, "x":3.50384, "y":2.68597, "heading":-0.9529, "vx":-1.07506, "vy":-0.89659, "omega":-0.88163, "ax":-2.0604, "ay":-1.41367, "alpha":0.11724, "fx":[-35.0548,-35.36458,-35.15449,-34.61319], "fy":[-23.53451,-23.95734,-24.5385,-24.1539]},
+    {"t":4.02379, "x":3.45941, "y":2.64916, "heading":-0.988, "vx":-1.15708, "vy":-0.95286, "omega":-0.87696, "ax":-2.0612, "ay":-1.41232, "alpha":0.19824, "fx":[-34.9589,-35.79319,-35.15059,-34.33927], "fy":[-23.33539,-23.69679,-24.79924,-24.26145]},
+    {"t":4.06359, "x":3.41172, "y":2.61011, "heading":-1.02291, "vx":-1.23913, "vy":-1.00909, "omega":-0.86907, "ax":-2.06214, "ay":-1.41076, "alpha":0.23154, "fx":[-34.98538,-35.78992,-35.30257,-34.22776], "fy":[-23.05908,-23.74604,-24.91051,-24.27109]},
+    {"t":4.1034, "x":3.36076, "y":2.56883, "heading":-1.0575, "vx":-1.32122, "vy":-1.06525, "omega":-0.85985, "ax":-2.06324, "ay":-1.40893, "alpha":0.3004, "fx":[-34.86779,-36.18702,-35.30968,-34.01583], "fy":[-22.93169,-23.43453,-25.10671,-24.38905]},
+    {"t":4.14321, "x":3.30653, "y":2.5253, "heading":-1.09173, "vx":-1.40336, "vy":-1.12133, "omega":-0.84789, "ax":-2.06454, "ay":-1.40674, "alpha":0.31423, "fx":[-34.89632,-36.09689,-35.49196,-33.98402], "fy":[-22.70167,-23.50265,-25.12681,-24.3821]},
+    {"t":4.18302, "x":3.24903, "y":2.47955, "heading":-1.12548, "vx":-1.48554, "vy":-1.17733, "omega":-0.83539, "ax":-2.06612, "ay":-1.40409, "alpha":0.37043, "fx":[-34.7742,-36.46544,-35.49966,-33.83738], "fy":[-22.63902,-23.15623,-25.24971,-24.48792]},
+    {"t":4.22282, "x":3.18825, "y":2.43157, "heading":-1.15874, "vx":-1.56779, "vy":-1.23322, "omega":-0.82064, "ax":-2.06807, "ay":-1.40081, "alpha":0.36117, "fx":[-34.83297,-36.26224,-35.7099,-33.90423], "fy":[-22.44986,-23.24777,-25.17096,-24.44096]},
+    {"t":4.26263, "x":3.1242, "y":2.38137, "heading":-1.19141, "vx":-1.65011, "vy":-1.28899, "omega":-0.80626, "ax":-2.07054, "ay":-1.39664, "alpha":0.40491, "fx":[-34.7202,-36.63028,-35.7017,-33.82499], "fy":[-22.44776,-22.86407,-25.20651,-24.50759]},
+    {"t":4.30244, "x":3.05688, "y":2.32895, "heading":-1.2235, "vx":-1.73254, "vy":-1.34459, "omega":-0.79014, "ax":-2.07376, "ay":-1.39117, "alpha":0.37036, "fx":[-34.83509,-36.3269,-35.92743,-34.00691], "fy":[-22.28335,-22.98214,-25.00621,-24.38195]},
+    {"t":4.34225, "x":2.98626, "y":2.27433, "heading":-1.25496, "vx":-1.81509, "vy":-1.39996, "omega":-0.7754, "ax":-2.07814, "ay":-1.38367, "alpha":0.39572, "fx":[-34.77164,-36.69055,-35.90806,-34.02428], "fy":[-22.31178,-22.57546,-24.90694,-24.34938]},
+    {"t":4.38205, "x":2.91236, "y":2.2175, "heading":-1.28582, "vx":-1.89782, "vy":-1.45505, "omega":-0.75965, "ax":-2.08445, "ay":-1.37277, "alpha":0.33112, "fx":[-34.99961,-36.30086,-36.1559,-34.36755], "fy":[-22.13271,-22.66722,-24.52932,-24.07262]},
+    {"t":4.42186, "x":2.83516, "y":2.15849, "heading":-1.31606, "vx":-1.98079, "vy":-1.50969, "omega":-0.74647, "ax":-2.09433, "ay":-1.35546, "alpha":0.3301, "fx":[-35.06924,-36.71132,-36.16121,-34.55384], "fy":[-22.08099,-22.19454,-24.15802,-23.79045]},
+    {"t":4.46167, "x":2.75465, "y":2.09732, "heading":-1.34578, "vx":-2.06416, "vy":-1.56365, "omega":-0.73333, "ax":-2.11193, "ay":-1.32377, "alpha":0.24352, "fx":[-35.5217,-36.54293,-36.47454,-35.1538], "fy":[-21.6601,-21.97859,-23.34403,-23.08539]},
+    {"t":4.50148, "x":2.67081, "y":2.03402, "heading":-1.37497, "vx":-2.14823, "vy":-1.61635, "omega":-0.72363, "ax":-2.15209, "ay":-1.24707, "alpha":0.19639, "fx":[-36.24325,-37.21673,-36.95199,-36.01393], "fy":[-20.7211,-20.55841,-21.86594,-21.70371]},
+    {"t":4.54129, "x":2.58359, "y":1.96869, "heading":-1.40378, "vx":-2.2339, "vy":-1.66599, "omega":-0.71581, "ax":-2.32149, "ay":-0.81154, "alpha":0.21178, "fx":[-38.71644,-41.5397,-39.06002,-38.63564], "fy":[-13.55867,-13.59136,-14.06609,-14.00008]},
+    {"t":4.58109, "x":2.49282, "y":1.90173, "heading":-1.43227, "vx":-2.32632, "vy":-1.6983, "omega":-0.70738, "ax":1.683, "ay":1.80591, "alpha":0.0154, "fx":[28.53798,28.73544,28.73516,28.50053], "fy":[30.26599,31.65407,30.48737,30.46465]},
+    {"t":4.6209, "x":2.40155, "y":1.83556, "heading":-1.46043, "vx":-2.25932, "vy":-1.62641, "omega":-0.70677, "ax":1.92383, "ay":1.5783, "alpha":-0.10329, "fx":[32.5752,32.68119,33.16871,32.46979], "fy":[26.41807,26.55049,27.23563,27.18146]},
+    {"t":4.66071, "x":2.31314, "y":1.77206, "heading":-1.48857, "vx":-2.18274, "vy":-1.56358, "omega":-0.71088, "ax":1.97398, "ay":1.52249, "alpha":-0.2228, "fx":[33.07642,34.24252,34.05685,32.93187], "fy":[25.39324,25.15147,26.54366,26.49989]},
+    {"t":4.70052, "x":2.22781, "y":1.71103, "heading":-1.51687, "vx":-2.10416, "vy":-1.50297, "omega":-0.71975, "ax":1.99552, "ay":1.49746, "alpha":-0.24253, "fx":[33.41023,34.4695,34.61842,33.27483], "fy":[24.73777,24.79985,26.17611,26.17167]},
+    {"t":4.74032, "x":2.14563, "y":1.65238, "heading":-1.54552, "vx":-2.02472, "vy":-1.44336, "omega":-0.72941, "ax":2.0075, "ay":1.48324, "alpha":-0.30895, "fx":[33.42202,35.01728,34.84714,33.30154], "fy":[24.56783,24.14112,26.0853,26.12397]},
+    {"t":4.78013, "x":2.06662, "y":1.5961, "heading":-1.57455, "vx":-1.94481, "vy":-1.38432, "omega":-0.74171, "ax":2.01512, "ay":1.47408, "alpha":-0.27883, "fx":[33.63668,34.75756,35.15048,33.56203], "fy":[24.26992,24.23911,25.84179,25.94405]},
+    {"t":4.81994, "x":1.9908, "y":1.54216, "heading":-1.60408, "vx":-1.86459, "vy":-1.32564, "omega":-0.75281, "ax":2.0204, "ay":1.46769, "alpha":-0.32126, "fx":[33.56905,35.22413,35.14011,33.53256], "fy":[24.31366,23.80502,25.79892,25.94221]},
+    {"t":4.85975, "x":1.91817, "y":1.49055, "heading":-1.63405, "vx":-1.78416, "vy":-1.26721, "omega":-0.76559, "ax":2.02427, "ay":1.46297, "alpha":-0.26712, "fx":[33.77197,34.87476,35.29558,33.78702], "fy":[24.17837,24.04696,25.55896,25.75442]},
+    {"t":4.89956, "x":1.84875, "y":1.44127, "heading":-1.66452, "vx":-1.70358, "vy":-1.20897, "omega":-0.77623, "ax":2.02723, "ay":1.45935, "alpha":-0.27639, "fx":[33.75688,35.17862,35.18841,33.80679], "fy":[24.30455,23.79897,25.48862,25.70009]},
+    {"t":4.93936, "x":1.78255, "y":1.3943, "heading":-1.69542, "vx":-1.62288, "vy":-1.15088, "omega":-0.78723, "ax":2.02957, "ay":1.45648, "alpha":-0.20624, "fx":[33.98325,34.81845,35.2238,34.06421], "fy":[24.27182,24.09778,25.25076,25.47656]},
+    {"t":4.97917, "x":1.71955, "y":1.34964, "heading":-1.72676, "vx":-1.54209, "vy":-1.0929, "omega":-0.79544, "ax":2.03146, "ay":1.45415, "alpha":-0.18566, "fx":[34.04579,34.98965,35.04719,34.13575], "fy":[24.43797,24.00219,25.15095,25.34734]},
+    {"t":5.01898, "x":1.65977, "y":1.30729, "heading":-1.75843, "vx":-1.46122, "vy":-1.03501, "omega":-0.80283, "ax":2.03303, "ay":1.45222, "alpha":-0.09915, "fx":[34.32377,34.6221,34.97775,34.40109], "fy":[24.46488,24.35807,24.91686,25.06738]},
+    {"t":5.05879, "x":1.60322, "y":1.26723, "heading":-1.79038, "vx":-1.38029, "vy":-0.9772, "omega":-0.80678, "ax":2.03434, "ay":1.45059, "alpha":-0.05228, "fx":[34.4665,34.71406,34.7274,34.50603], "fy":[24.64776,24.39968,24.79726,24.85208]},
+    {"t":5.09859, "x":1.54988, "y":1.22948, "heading":-1.8225, "vx":-1.29931, "vy":-0.91946, "omega":-0.80886, "ax":2.03546, "ay":1.44921, "alpha":0.05131, "fx":[34.81274,34.35263,34.55122,34.77347], "fy":[24.70703,24.81864,24.57504,24.50185]},
+    {"t":5.1384, "x":1.49977, "y":1.19403, "heading":-1.8547, "vx":-1.21828, "vy":-0.86177, "omega":-0.80682, "ax":2.03642, "ay":1.44801, "alpha":0.12266, "fx":[35.043,34.39206,34.22125,34.89929], "fy":[24.89644,24.97658,24.45279,24.19545]},
+    {"t":5.17821, "x":1.45289, "y":1.16087, "heading":-1.88682, "vx":-1.13721, "vy":-0.80413, "omega":-0.80193, "ax":2.03726, "ay":1.44697, "alpha":0.24657, "fx":[35.47309,34.04916,33.92965,35.16081], "fy":[24.96071,25.48574,24.24716,23.75672]},
+    {"t":5.21802, "x":1.40923, "y":1.13001, "heading":-1.91874, "vx":-1.05612, "vy":-0.74653, "omega":-0.79212, "ax":2.038, "ay":1.44606, "alpha":0.34466, "fx":[35.80202,34.04918,33.51266,35.29902], "fy":[25.13867,25.77557,24.13041,23.34333]},
+    {"t":5.25783, "x":1.36881, "y":1.10144, "heading":-1.95027, "vx":-0.97499, "vy":-0.68896, "omega":-0.7784, "ax":2.03865, "ay":1.44524, "alpha":0.49175, "fx":[36.33474,33.72443,33.09793,35.55022], "fy":[25.19503,26.37788,23.95492,22.80485]},
+    {"t":5.29763, "x":1.33161, "y":1.07515, "heading":-1.98126, "vx":-0.89383, "vy":-0.63143, "omega":-0.75882, "ax":2.03923, "ay":1.44452, "alpha":0.618, "fx":[36.77064,33.71278,32.57475,35.68878], "fy":[25.35378,26.79674,23.85923,22.2736]},
+    {"t":5.33744, "x":1.29764, "y":1.05116, "heading":-2.01147, "vx":-0.81266, "vy":-0.57393, "omega":-0.73422, "ax":2.03975, "ay":1.44387, "alpha":0.79308, "fx":[37.41995,33.42095,32.02206,35.91955], "fy":[25.38622,27.51742,23.72024,21.61514]},
+    {"t":5.37725, "x":1.26691, "y":1.02946, "heading":-2.04069, "vx":-0.73146, "vy":-0.51645, "omega":-0.70265, "ax":2.04023, "ay":1.44328, "alpha":0.95269, "fx":[37.98202,33.40074,31.37385,36.05799], "fy":[25.51644,28.08097,23.65525,20.94636]},
+    {"t":5.41706, "x":1.23941, "y":1.01005, "heading":-2.06866, "vx":-0.65024, "vy":-0.459, "omega":-0.66473, "ax":2.04065, "ay":1.44274, "alpha":1.16265, "fx":[38.77401,33.13707,30.66248,36.27014], "fy":[25.51589,28.93988,23.56138,20.14555]},
+    {"t":5.45686, "x":1.21514, "y":0.99292, "heading":-2.09513, "vx":-0.56901, "vy":-0.40156, "omega":-0.61844, "ax":2.04104, "ay":1.44226, "alpha":1.36128, "fx":[39.4762,33.12354,29.86596,36.40451], "fy":[25.61076,29.66997,23.53268,19.3162]},
+    {"t":5.49667, "x":1.19411, "y":0.97807, "heading":-2.11974, "vx":-0.48776, "vy":-0.34415, "omega":-0.56425, "ax":2.0414, "ay":1.44181, "alpha":1.61386, "fx":[40.43225,32.89284,28.96625,36.60313], "fy":[25.58058,30.68718,23.48762,18.34393]},
+    {"t":5.53648, "x":1.17631, "y":0.96552, "heading":-2.14221, "vx":-0.40649, "vy":-0.28676, "omega":-0.50001, "ax":2.04173, "ay":1.4414, "alpha":1.86022, "fx":[41.30117,32.87754,27.99376,36.74428], "fy":[25.64782,31.61108,23.49156,17.32102]},
+    {"t":5.57629, "x":1.16174, "y":0.95524, "heading":-2.16211, "vx":-0.32522, "vy":-0.22938, "omega":-0.42596, "ax":2.04203, "ay":1.44103, "alpha":2.16598, "fx":[42.45634,32.66348,26.87274,36.94471], "fy":[25.59567,32.82067,23.489,16.14048]},
+    {"t":5.6161, "x":1.15041, "y":0.94725, "heading":-2.17907, "vx":-0.24393, "vy":-0.17201, "omega":-0.33974, "ax":2.04231, "ay":1.44068, "alpha":2.47068, "fx":[43.5177,32.63406,25.68127,37.12322], "fy":[25.65892,33.97083,23.5101,14.88224]},
+    {"t":5.6559, "x":1.14232, "y":0.94155, "heading":-2.19259, "vx":-0.16263, "vy":-0.11466, "omega":-0.24138, "ax":2.04257, "ay":1.44035, "alpha":2.84272, "fx":[44.90746,32.40369,24.29904,37.36365], "fy":[25.61645,35.41983,23.52135,13.44244]},
+    {"t":5.69571, "x":1.13747, "y":0.93813, "heading":-2.2022, "vx":-0.08132, "vy":-0.05733, "omega":-0.12822, "ax":2.04281, "ay":1.44005, "alpha":3.221, "fx":[46.19876,32.31761,22.83242,37.64141], "fy":[25.72043,36.8531,23.5139,11.89219]},
+    {"t":5.73552, "x":1.13585, "y":0.93698, "heading":-2.2073, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.9445, "ay":1.56924, "alpha":3.76401, "fx":[46.65459,30.20365,18.99165,36.45184], "fy":[27.96657,40.97435,25.71061,12.11765]},
+    {"t":5.77526, "x":1.13738, "y":0.93822, "heading":-2.2073, "vx":0.07728, "vy":0.06236, "omega":0.14959, "ax":1.94498, "ay":1.56962, "alpha":3.27712, "fx":[44.84402,30.63086,20.84229,36.01716], "fy":[27.86082,39.15546,25.82847,13.95072]},
+    {"t":5.815, "x":1.14199, "y":0.94194, "heading":-2.20136, "vx":0.15457, "vy":0.12474, "omega":0.27982, "ax":1.94496, "ay":1.56961, "alpha":2.88362, "fx":[43.50018,30.76698,22.37428,35.69149], "fy":[27.70963,37.68038,25.85776,15.54655]},
+    {"t":5.85474, "x":1.14967, "y":0.94814, "heading":-2.19024, "vx":0.23187, "vy":0.18712, "omega":0.39442, "ax":1.94494, "ay":1.56959, "alpha":2.49848, "fx":[42.05857,31.02156,23.82001,35.43126], "fy":[27.74147,36.18869,25.84725,17.01569]},
+    {"t":5.89448, "x":1.16042, "y":0.95681, "heading":-2.17456, "vx":0.30916, "vy":0.2495, "omega":0.49371, "ax":1.94491, "ay":1.56957, "alpha":2.18029, "fx":[40.94655,31.06498,25.07761,35.24061], "fy":[27.66395,35.00031,25.81224,18.31523]},
+    {"t":5.93422, "x":1.17424, "y":0.96797, "heading":-2.15494, "vx":0.38645, "vy":0.31187, "omega":0.58036, "ax":1.94489, "ay":1.56955, "alpha":1.86298, "fx":[39.74225,31.28585,26.25462,35.04523], "fy":[27.72617,33.7452,25.79728,19.52162]},
+    {"t":5.97396, "x":1.19114, "y":0.9816, "heading":-2.13188, "vx":0.46374, "vy":0.37425, "omega":0.65439, "ax":1.94486, "ay":1.56952, "alpha":1.60556, "fx":[38.83144,31.30472,27.28479,34.90504], "fy":[27.65358,32.78756,25.76848,20.57902]},
+    {"t":6.01371, "x":1.2111, "y":0.99771, "heading":-2.10587, "vx":0.54103, "vy":0.43662, "omega":0.7182, "ax":1.94483, "ay":1.5695, "alpha":1.34285, "fx":[37.83137,31.53863,28.2363,34.71751], "fy":[27.69737,31.72424,25.79331,21.57195]},
+    {"t":6.05345, "x":1.23414, "y":1.01631, "heading":-2.07733, "vx":0.61832, "vy":0.499, "omega":0.77157, "ax":1.94479, "ay":1.56947, "alpha":1.13568, "fx":[37.09502,31.558,29.07985,34.58854], "fy":[27.59512,30.96632,25.79777,22.42569]},
+    {"t":6.09319, "x":1.26025, "y":1.03738, "heading":-2.04667, "vx":0.69561, "vy":0.56137, "omega":0.8167, "ax":1.94475, "ay":1.56944, "alpha":0.91738, "fx":[36.2711,31.82876,29.83638,34.38251], "fy":[27.60338,30.06235,25.87613,23.24088]},
+    {"t":6.13293, "x":1.28943, "y":1.06092, "heading":-2.01421, "vx":0.7729, "vy":0.62374, "omega":0.85316, "ax":1.94471, "ay":1.5694, "alpha":0.75239, "fx":[35.68856,31.85242,30.52172,34.25308], "fy":[27.45732,29.48018,25.92166,23.92112]},
+    {"t":6.17267, "x":1.32168, "y":1.08695, "heading":-1.98031, "vx":0.85018, "vy":0.68611, "omega":0.88306, "ax":1.94466, "ay":1.56936, "alpha":0.57031, "fx":[35.01887,32.16718,31.10856,34.01782], "fy":[27.42464,28.71412,26.05247,24.58632]},
+    {"t":6.21241, "x":1.357, "y":1.11546, "heading":-1.94521, "vx":0.92747, "vy":0.74848, "omega":0.90572, "ax":1.9446, "ay":1.56932, "alpha":0.44147, "fx":[34.57733,32.19166,31.6577,33.88195], "fy":[27.23933,28.28842,26.13214,25.11454]},
+    {"t":6.25215, "x":1.39539, "y":1.14644, "heading":-1.90922, "vx":1.00475, "vy":0.81084, "omega":0.92327, "ax":1.94454, "ay":1.56926, "alpha":0.28956, "fx":[34.03955,32.54311,32.09399,33.62764], "fy":[27.17591,27.64139,26.30557,25.64802]},
+    {"t":6.29189, "x":1.43686, "y":1.1799, "heading":-1.87253, "vx":1.08202, "vy":0.87321, "omega":0.93477, "ax":1.94447, "ay":1.5692, "alpha":0.19305, "fx":[33.72467,32.55063,32.52788,33.49612], "fy":[26.96116,27.36331,26.4042,26.0381]},
+    {"t":6.33163, "x":1.4814, "y":1.21585, "heading":-1.83538, "vx":1.1593, "vy":0.93557, "omega":0.94245, "ax":1.94438, "ay":1.56913, "alpha":0.06716, "fx":[33.30518,32.93137,32.82564,33.23127], "fy":[26.88234,26.81973,26.60566,26.45428]},
+    {"t":6.37137, "x":1.529, "y":1.25427, "heading":-1.79793, "vx":1.23657, "vy":0.99793, "omega":0.94512, "ax":1.94428, "ay":1.56905, "alpha":0.00079, "fx":[33.10785,32.89733,33.16302,33.11836], "fy":[26.65265,26.68144,26.70457,26.71768]},
+    {"t":6.41112, "x":1.57968, "y":1.29516, "heading":-1.76037, "vx":1.31384, "vy":1.06028, "omega":0.94515, "ax":1.94416, "ay":1.56895, "alpha":-0.10122, "fx":[32.79247,33.29816,33.33003,32.85761], "fy":[26.5767,26.23328,26.91463,27.02498]},
+    {"t":6.45086, "x":1.63343, "y":1.33854, "heading":-1.72281, "vx":1.3911, "vy":1.12263, "omega":0.94112, "ax":1.94401, "ay":1.56883, "alpha":-0.13774, "fx":[32.70713,33.20286,33.57416,32.78403], "fy":[26.3517,26.22326,26.9955,27.1708]},
+    {"t":6.4906, "x":1.69025, "y":1.38439, "heading":-1.6854, "vx":1.46836, "vy":1.18498, "omega":0.93565, "ax":1.94382, "ay":1.56868, "alpha":-0.21939, "fx":[32.47995,33.60663,33.62701,32.54191], "fy":[26.31501,25.82853,27.20078,27.38661]},
+    {"t":6.53034, "x":1.75014, "y":1.43272, "heading":-1.64822, "vx":1.54561, "vy":1.24732, "omega":0.92693, "ax":1.94358, "ay":1.56848, "alpha":-0.2212, "fx":[32.50615,33.40164,33.79564,32.53583], "fy":[26.10278,25.97962,27.22686,27.4083]},
+    {"t":6.57008, "x":1.8131, "y":1.48353, "heading":-1.61138, "vx":1.62285, "vy":1.30965, "omega":0.91814, "ax":1.94327, "ay":1.56822, "alpha":-0.2824, "fx":[32.3507,33.80633,33.73208,32.32844], "fy":[26.1373,25.61884,27.40407,27.53961]},
+    {"t":6.60982, "x":1.87912, "y":1.53682, "heading":-1.5749, "vx":1.70007, "vy":1.37198, "omega":0.90692, "ax":1.94282, "ay":1.56785, "alpha":-0.24632, "fx":[32.47885,33.47217,33.8233,32.41288], "fy":[25.96125,25.93407,27.34357,27.43603]},
+    {"t":6.64956, "x":1.94822, "y":1.59258, "heading":-1.53885, "vx":1.77728, "vy":1.43428, "omega":0.89713, "ax":1.94215, "ay":1.56731, "alpha":-0.27603, "fx":[32.39103,33.82148,33.65499,32.27407], "fy":[26.07231,25.68479,27.42673,27.45385]},
+    {"t":6.6893, "x":2.02038, "y":1.65081, "heading":-1.5032, "vx":1.85447, "vy":1.49657, "omega":0.88616, "ax":1.94103, "ay":1.56639, "alpha":-0.20408, "fx":[32.59628,33.36147,33.64639,32.46159], "fy":[25.99011,26.07466,27.26481,27.24577]},
+    {"t":6.72904, "x":2.09562, "y":1.71153, "heading":-1.46798, "vx":1.9316, "vy":1.55882, "omega":0.87805, "ax":1.93881, "ay":1.56455, "alpha":-0.19158, "fx":[32.56516,33.55527,33.36902,32.42484], "fy":[26.19954,25.93065,27.18789,27.13221]},
+    {"t":6.76878, "x":2.17391, "y":1.77471, "heading":-1.43309, "vx":2.00865, "vy":1.621, "omega":0.87043, "ax":1.93134, "ay":1.56, "alpha":-0.0913, "fx":[32.67225,33.0558,33.09505,32.58288], "fy":[26.21784,26.32549,26.82629,26.77113]},
+    {"t":6.80852, "x":2.25526, "y":1.84036, "heading":-1.3985, "vx":2.08541, "vy":1.68299, "omega":0.86681, "ax":-0.0001, "ay":0.00114, "alpha":0.00165, "fx":[0.00199,-0.00691,-0.0054,0.0035], "fy":[0.03182,0.01417,0.0147,0.01719]},
+    {"t":6.84827, "x":2.33814, "y":1.90725, "heading":-1.36405, "vx":2.0854, "vy":1.68304, "omega":0.86687, "ax":-1.93118, "ay":-1.5602, "alpha":0.09359, "fx":[-32.68835,-33.06378,-33.08651,-32.5564], "fy":[-26.19597,-26.34404,-26.85434,-26.75997]},
+    {"t":6.88801, "x":2.41949, "y":1.9729, "heading":-1.3296, "vx":2.00866, "vy":1.62103, "omega":0.87059, "ax":-1.93873, "ay":-1.56465, "alpha":0.19441, "fx":[-32.63865,-33.6169,-33.29215,-32.36125], "fy":[-26.13243,-25.9795,-27.2674,-27.07759]},
+    {"t":6.92775, "x":2.49778, "y":2.03609, "heading":-1.295, "vx":1.93161, "vy":1.55885, "omega":0.87832, "ax":-1.94098, "ay":-1.56646, "alpha":0.20524, "fx":[-32.74027,-33.46287,-33.50988,-32.34911], "fy":[-25.8647,-26.21479,-27.39107,-27.10925]},
+    {"t":6.96749, "x":2.57301, "y":2.0968, "heading":-1.2601, "vx":1.85447, "vy":1.4966, "omega":0.88647, "ax":-1.94211, "ay":-1.56735, "alpha":0.27618, "fx":[-32.61955,-33.98054,-33.42644,-32.11249], "fy":[-25.87952,-25.89532,-27.61551,-27.25048]},
+    {"t":7.00723, "x":2.64518, "y":2.15504, "heading":-1.22487, "vx":1.77729, "vy":1.43431, "omega":0.89745, "ax":-1.94279, "ay":-1.56789, "alpha":0.24646, "fx":[-32.77263,-33.66717,-33.54082,-32.20454], "fy":[-25.71862,-26.21347,-27.58524,-27.16012]},
+    {"t":7.04697, "x":2.71428, "y":2.2108, "heading":-1.1892, "vx":1.70008, "vy":1.372, "omega":0.90724, "ax":-1.94324, "ay":-1.56825, "alpha":0.28044, "fx":[-32.70356,-34.04372,-33.38025,-32.08829], "fy":[-25.84685,-25.94553,-27.68789,-27.22168]},
+    {"t":7.08671, "x":2.7803, "y":2.26409, "heading":-1.15315, "vx":1.62286, "vy":1.30968, "omega":0.91839, "ax":-1.94356, "ay":-1.56851, "alpha":0.22121, "fx":[-32.87675,-33.64863,-33.43567,-32.27675], "fy":[-25.79852,-26.33049,-27.52964,-27.06072]},
+    {"t":7.12645, "x":2.84326, "y":2.3149, "heading":-1.11665, "vx":1.54562, "vy":1.24735, "omega":0.92718, "ax":-1.9438, "ay":-1.5687, "alpha":0.21759, "fx":[-32.84645,-33.85347,-33.26147,-32.29283], "fy":[-26.01167,-26.16728,-27.49626,-27.0573]},
+    {"t":7.16619, "x":2.90315, "y":2.36323, "heading":-1.0798, "vx":1.46837, "vy":1.185, "omega":0.93583, "ax":-1.94399, "ay":-1.56885, "alpha":0.13862, "fx":[-33.00581,-33.41694,-33.27968,-32.56461], "fy":[-26.09842,-26.50908,-27.24791,-26.88725]},
+    {"t":7.20593, "x":2.95997, "y":2.40908, "heading":-1.04261, "vx":1.39111, "vy":1.12266, "omega":0.94134, "ax":-1.94414, "ay":-1.56897, "alpha":0.10068, "fx":[-32.9918,-33.44627,-33.12921,-32.70998], "fy":[-26.40624,-26.39269,-27.0917,-26.8602]},
+    {"t":7.24567, "x":3.01372, "y":2.45246, "heading":-1.0052, "vx":1.31385, "vy":1.0603, "omega":0.94534, "ax":-1.94427, "ay":-1.56907, "alpha":-0.00035, "fx":[-33.12576,-32.93621,-33.14782,-33.07585], "fy":[-26.61376,-26.70273,-26.74439,-26.69659]},
+    {"t":7.28542, "x":3.0644, "y":2.49336, "heading":-0.96763, "vx":1.23658, "vy":0.99795, "omega":0.94532, "ax":-1.94437, "ay":-1.56915, "alpha":-0.06921, "fx":[-33.09403,-32.82471,-33.03781,-33.33606], "fy":[-27.01792,-26.61457,-26.46089,-26.66968]},
+    {"t":7.32516, "x":3.11201, "y":2.53178, "heading":-0.93007, "vx":1.15931, "vy":0.93559, "omega":0.94257, "ax":-1.94446, "ay":-1.56922, "alpha":-0.19239, "fx":[-33.18053,-32.24019,-33.08026,-33.79756], "fy":[-27.34918,-26.86281,-26.0159,-26.53984]},
+    {"t":7.3649, "x":3.15654, "y":2.56772, "heading":-0.89261, "vx":1.08204, "vy":0.87323, "omega":0.93493, "ax":-1.94453, "ay":-1.56928, "alpha":-0.29225, "fx":[-33.10823,-31.99223,-33.03346,-34.16965], "fy":[-27.86109,-26.76068,-25.60649,-26.54351]},
+    {"t":7.40464, "x":3.19801, "y":2.60118, "heading":-0.85545, "vx":1.00476, "vy":0.81086, "omega":0.92331, "ax":-1.94459, "ay":-1.56933, "alpha":-0.44144, "fx":[-33.13038,-31.31423,-33.12712,-34.73625], "fy":[-28.31482,-26.94589,-25.05134,-26.4632]},
+    {"t":7.44438, "x":3.2364, "y":2.63217, "heading":-0.81876, "vx":0.92748, "vy":0.7485, "omega":0.90577, "ax":-1.94465, "ay":-1.56937, "alpha":-0.57379, "fx":[-32.9934,-30.93835,-33.16305,-35.217], "fy":[-28.94373,-26.80553,-24.50852,-26.52055]},
+    {"t":7.48412, "x":3.27173, "y":2.66068, "heading":-0.78276, "vx":0.8502, "vy":0.68613, "omega":0.88297, "ax":-1.9447, "ay":-1.56941, "alpha":-0.75167, "fx":[-32.92593,-30.17115,-33.32713,-35.89099], "fy":[-29.52785,-26.90887,-23.83364,-26.51065]},
+    {"t":7.52386, "x":3.30398, "y":2.6867, "heading":-0.74767, "vx":0.77292, "vy":0.62376, "omega":0.85309, "ax":-1.94474, "ay":-1.56945, "alpha":-0.92195, "fx":[-32.71058,-29.64641,-33.47158,-36.48961], "fy":[-30.28649,-26.70817,-23.14632,-26.64244]},
+    {"t":7.5636, "x":3.33316, "y":2.71025, "heading":-0.71377, "vx":0.69563, "vy":0.56139, "omega":0.81646, "ax":-1.94478, "ay":-1.56948, "alpha":-1.13455, "fx":[-32.53622,-28.77288,-33.72882,-37.28298], "fy":[-31.01301,-26.71506,-22.33687,-26.72062]},
+    {"t":7.60334, "x":3.35927, "y":2.73132, "heading":-0.68132, "vx":0.61834, "vy":0.49901, "omega":0.77137, "ax":-1.94482, "ay":-1.56951, "alpha":-1.34844, "fx":[-32.22644,-28.08714,-34.00133,-38.00841], "fy":[-31.9187,-26.43357,-21.4915,-26.94371]},
+    {"t":7.64308, "x":3.38231, "y":2.74992, "heading":-0.65067, "vx":0.54105, "vy":0.43664, "omega":0.71778, "ax":-1.94485, "ay":-1.56953, "alpha":-1.60489, "fx":[-31.93412,-27.07317,-34.37526,-38.94297], "fy":[-32.80802,-26.33579,-20.52252,-27.1229]},
+    {"t":7.68282, "x":3.40227, "y":2.76603, "heading":-0.62214, "vx":0.46376, "vy":0.37427, "omega":0.654, "ax":-1.94488, "ay":-1.56956, "alpha":-1.86985, "fx":[-31.5169,-26.21351,-34.78896,-39.80814], "fy":[-33.88848,-25.962,-19.49113,-27.44922]},
+    {"t":7.72257, "x":3.41917, "y":2.77966, "heading":-0.59615, "vx":0.38647, "vy":0.31189, "omega":0.57969, "ax":-1.94491, "ay":-1.56958, "alpha":-2.1795, "fx":[-31.09937,-25.02908,-35.29232,-40.90856], "fy":[-34.9698,-25.75388,-18.33463,-27.73395]},
+    {"t":7.76231, "x":3.43299, "y":2.79082, "heading":-0.57312, "vx":0.30918, "vy":0.24951, "omega":0.49307, "ax":-1.94493, "ay":-1.5696, "alpha":-2.50685, "fx":[-30.57878,-23.94843,-35.85567,-41.9481], "fy":[-36.26332,-25.28835,-17.07956,-28.16237]},
+    {"t":7.80205, "x":3.44374, "y":2.79949, "heading":-0.55352, "vx":0.23189, "vy":0.18714, "omega":0.39345, "ax":-1.94496, "ay":-1.56962, "alpha":-2.88299, "fx":[-30.04521,-22.53737,-36.48976,-43.26019], "fy":[-37.58391,-24.98555,-15.68603,-28.53933]},
+    {"t":7.84179, "x":3.45142, "y":2.80569, "heading":-0.53788, "vx":0.15459, "vy":0.12476, "omega":0.27888, "ax":-1.94498, "ay":-1.56963, "alpha":-3.28685, "fx":[-29.44528,-21.16994,-37.19453,-44.5242], "fy":[-39.15312,-24.45258,-14.1466,-29.04365]},
+    {"t":7.88153, "x":3.45603, "y":2.80941, "heading":-0.5268, "vx":0.0773, "vy":0.06238, "omega":0.14826, "ax":-1.945, "ay":-1.56965, "alpha":-3.73054, "fx":[-28.87863,-19.5059,-37.95901,-45.99171], "fy":[-40.71554,-24.08145,-12.45309,-29.54691]},
+    {"t":7.92127, "x":3.45756, "y":2.81065, "heading":-0.52091, "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,77,93,151]
+ },
+ "events":[]
+}
diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index b513bc9..6fd0987 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -462,5 +462,85 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
     sleep(Duration::from_secs_f64(1.5)).await;
     indexer.stop();
 
+    Ok(())
+}
+
+pub async fn right_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error::Error>> {
+    let mut robot_ref = robot.borrow_mut();
+    let mut drivetrain = robot_ref.drivetrain.deref().borrow_mut();
+    let mut elevator = robot_ref.elevator.deref().borrow_mut();
+    let mut indexer = robot_ref.indexer.deref().borrow_mut();
+
+    drivetrain.reset_heading_offset(
+        if alliance_station().red() {
+            Angle::new::<degree>(180.)
+        } else {
+            Angle::new::<degree>(0.)
+        });
+    drivetrain.odometry.set(Vector2::new(
+        Length::new::<meter>(7.18402099609375),
+        Length::new::<meter>(2.696911096572876),
+    ));
+
+    join!(drive("Right2", &mut drivetrain, 1), async {
+        elevator.set_target(ElevatorPosition::L4);
+        elevator.run_to_target_trapezoid();
+    });
+
+    let _ = timeout(Duration::from_secs_f64(0.5), async {
+        loop {
+            drivetrain.update_limelight().await;
+            drivetrain.post_odo().await;
+
+            sleep(Duration::from_millis(20)).await;
+        }
+    }).await;
+
+    async_score(
+        &mut drivetrain,
+        LineupSide::Right,
+        &mut elevator,
+        &mut indexer,
+        ElevatorPosition::L4,
+        robot_ref.dt,
+        if alliance_station().red() {Some(8)} else { Some(17)}
+    )
+        .await;
+
+    join!(drive("Right2", &mut drivetrain, 3), async {
+        elevator.set_target(ElevatorPosition::Bottom);
+        elevator.run_to_target_trapezoid_async().await;
+
+        indexer.set_speed(INTAKE_SPEED);
+
+        wait(|| indexer.is_laser_tripped()).await;
+
+        indexer.stop();
+    });
+
+    join!(drive("Right2", &mut drivetrain, 4), async {
+        sleep(Duration::from_secs_f64(1.25)).await;
+        elevator.set_target(ElevatorPosition::L4);
+        elevator.run_to_target_trapezoid();
+    });
+
+    let _ = timeout(Duration::from_secs_f64(0.5), async {
+        loop {
+            drivetrain.update_limelight().await;
+            sleep(Duration::from_millis(20)).await;
+        }
+    }).await;
+
+    async_score(
+        &mut drivetrain,
+        LineupSide::Left,
+        &mut elevator,
+        &mut indexer,
+        ElevatorPosition::L4,
+        robot_ref.dt,
+        if alliance_station().red() {Some(8)} else { Some(17)}
+    )
+        .await;
+
     Ok(())
 }
\ No newline at end of file

From bcedd7af5779ddb283c8cbc8ccf7e7b85d132433 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 28 Mar 2025 11:44:54 -0500
Subject: [PATCH 10/14] added right auto to dashboard

---
 src/auto/mod.rs | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 6fd0987..e1aef7d 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -29,6 +29,7 @@ pub enum Auto {
     RotationTest,
     BlueMidLeft2,
     TushPush1,
+    Right2,
 }
 
 impl Auto {
@@ -42,6 +43,7 @@ impl Auto {
             "RotationTest" => Auto::RotationTest,
             "BlueMidLeft2" => Auto::BlueMidLeft2,
             "TushPush1" => Auto::TushPush1,
+            "Right2" => Auto::Right2,
             _ => Auto::Nothing,
         }
     }
@@ -56,6 +58,7 @@ impl Auto {
             Auto::RotationTest => "RotationTest",
             Auto::BlueMidLeft2 => "BlueMidLeft2",
             Auto::TushPush1 => "TushPush1",
+            Auto::Right2 => "Right2",
             _ => "none",
         }
     }
@@ -70,6 +73,7 @@ impl Auto {
             // Auto::RotationTest,
             Auto::BlueMidLeft2,
             Auto::TushPush1,
+            Auto::Right2,
         ]
     }
 
@@ -96,6 +100,7 @@ impl Auto {
             Auto::RotationTest => rotation_test(Rc::clone(&ferris)).await.expect("Failed running auto"),
             Auto::BlueMidLeft2 => blue_mid_left_2(Rc::clone(&ferris)).await.expect("Failed running auto"),
             Auto::TushPush1 => tush_push_1(Rc::clone(&ferris)).await.expect("Failed running auto"),
+            Auto::Right2 => right_2(Rc::clone(&ferris)).await.expect("Failed running auto"),
         }
     }
 }

From eb3cb6b71fa1de401b36b95125ac92782a6baeff Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 28 Mar 2025 11:58:02 -0500
Subject: [PATCH 11/14] tuned lineup

---
 src/subsystems/drivetrain.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index 0d53712..1c80bee 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -649,8 +649,8 @@ impl Drivetrain {
             error_position *= -LINEUP_DRIVE_KP;
 
             // Give KP boost when close
-            if error_position.magnitude().abs() < 0.15 {
-                error_position *= 2.25;
+            if error_position.magnitude().abs() < 0.2 {
+                error_position *= 2.5;
             }
 
             let mut speed = error_position;

From 5395b94f15135f3b779a22bb732c801d03a6d934 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Fri, 28 Mar 2025 16:23:11 -0500
Subject: [PATCH 12/14] added override to score

---
 src/main.rs | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index aa3ac0a..0402d9d 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -161,7 +161,7 @@ async fn teleop(robot: &mut Ferris) {
             if let Ok(mut indexer) = robot.indexer.try_borrow_mut() {
                 drivetrain.update_limelight().await;
                 drivetrain.post_odo().await;
-                
+
                 let drivetrain_aligned = robot.debouncer.calculate(if robot.controllers.right_drive.get(LINEUP_LEFT) {
                     drivetrain
                         .lineup(LineupSide::Left, elevator.get_target(), robot.dt, None)
@@ -184,7 +184,9 @@ async fn teleop(robot: &mut Ferris) {
                     false
                 });
 
-                if robot.controllers.left_drive.get_pov() != -1 {
+                if robot.controllers.right_drive.get_pov() != -1 {
+                    indexer.set_speed(-0.5);
+                } else if robot.controllers.left_drive.get_pov() != -1 {
                     elevator.set_target(ElevatorPosition::L3Algae);
                     elevator.run_to_target_trapezoid();
 

From 83df134ffc63b1861273f5aa909503feca888dde Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Sat, 29 Mar 2025 09:49:12 -0500
Subject: [PATCH 13/14] upped intake speed

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

diff --git a/src/constants.rs b/src/constants.rs
index 53ca1c8..834d57b 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -132,7 +132,7 @@ pub mod elevator {
 pub mod indexer {
     pub const LASER_TRIP_DISTANCE_MM: i32 = 10;
     pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08;
-    pub const INTAKE_SPEED: f64 = -0.25;
+    pub const INTAKE_SPEED: f64 = -0.45;
     pub const BOTTOM_SPEED: f64 = -0.35;
     pub const L2_SPEED: f64 = -0.4;
     pub const L3_SPEED: f64 = -0.5;

From 4a56206065c072a9586df55b380ab895b3c6fcf6 Mon Sep 17 00:00:00 2001
From: Nolan Peterson <ndpete2007@gmail.com>
Date: Sat, 29 Mar 2025 09:49:29 -0500
Subject: [PATCH 14/14] upped L3 speed

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

diff --git a/src/constants.rs b/src/constants.rs
index 834d57b..9e5b17e 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -135,7 +135,7 @@ pub mod indexer {
     pub const INTAKE_SPEED: f64 = -0.45;
     pub const BOTTOM_SPEED: f64 = -0.35;
     pub const L2_SPEED: f64 = -0.4;
-    pub const L3_SPEED: f64 = -0.5;
+    pub const L3_SPEED: f64 = -0.6;
     pub const L4_SPEED: f64 = -0.4;
 }
 pub mod climber {