Dev

.gitignore

@@ -1,2 +1,4 @@
 target/
 Cargo.lock
+.DS_Store
+.idea/

Cargo.toml

@@ -4,14 +4,14 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
-frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
-#frcrs = { path = "../../frcrs" }
+#frcrs = { git = "https://github.com/Team-2502/frcrs.git" }
+frcrs = { path = "../frcrs" }
 tokio = { version = "1.36.0", features = ["rt", "full"] }
 serde = { version = "1.0.217", features = ["derive"] }
 serde_json = "1.0.134"
 uom = { version = "0.35.0", features = ["f64"] }
 nalgebra = "0.32.3"
-wpi-trajectory = { git = "https://github.com/Speedy6451/trajectory-rs" }
+wpi-trajectory = { git = "https://github.com/Sha-dos/trajectory-rs" }
 axum = "0.7.4"
 # limelightlib-rust = { git = "https://github.com/LimelightVision/limelightlib-rust.git" }
 

Makefile

@@ -2,6 +2,7 @@ LIB=RobotCode2025
 OUT=target/arm-unknown-linux-gnueabi/release/$(LIB)
 DEPLOY=javastub/src/main/deploy/$(LIB)
 TEAM=25.02
+PATHS=auto/*
 
 .PHONY: check
 check:
@@ -27,6 +28,10 @@ deploy-scp: $(OUT)
 	scp $(OUT) lvuser@10.$(TEAM).2:
 	ssh lvuser@10.$(TEAM).2 /usr/local/frc/bin/frcRunRobot.sh
 
+.PHONY: deploy-paths
+deploy-paths:
+	scp -r $(PATHS) admin@10.$(TEAM).2:/home/lvuser/deploy/choreo/
+
 # Deploys the "deploy" directory and robotcode
 .PHONY: deploy
 deploy: $(OUT)

auto/paths.chor

@@ -0,0 +1,78 @@
+{
+ "name":"paths",
+ "version":1,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11 in",
+    "val":0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11 in",
+    "val":-0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "mass":{
+   "exp":"150 lbs",
+   "val":68.0388555
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.5",
+   "val":6.5
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"1.2 N * m",
+   "val":1.2
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "side":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "back":{
+    "exp":"16 in",
+    "val":0.4064
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[]
+}

src/auto/mod.rs

@@ -1,31 +1,39 @@
 mod path;
 
+use crate::auto::path::drive;
+use nalgebra::Vector2;
 use serde::{Deserialize, Serialize};
+use std::ops::Deref;
+use std::time::Duration;
+use tokio::time::sleep;
 
 use crate::Ferris;
 
 #[derive(Serialize, Deserialize)]
 pub enum Auto {
     Nothing,
+    BlueTriangle,
 }
 
 impl Auto {
     pub fn from_dashboard(s: &str) -> Self {
         match s {
             "Nothing" => Auto::Nothing,
+            "BlueTriangle" => Auto::BlueTriangle,
             _ => Auto::Nothing,
         }
     }
 
     pub fn name(&self) -> &'static str {
         match self {
             Auto::Nothing => "Nothing",
+            Auto::BlueTriangle => "BlueTriangle",
             _ => "none",
         }
     }
 
     pub fn iterator() -> Vec<Self> {
-        vec![Auto::Nothing]
+        vec![Auto::Nothing, Auto::BlueTriangle]
     }
 
     pub fn names() -> Vec<String> {
@@ -35,9 +43,37 @@ impl Auto {
             .collect()
     }
 
-    pub async fn run_auto<'a>(_ferris: Ferris, chosen: Auto) {
+    pub async fn run_auto<'a>(ferris: Ferris, chosen: Auto) {
         match chosen {
             Auto::Nothing => {}
+            Auto::BlueTriangle => {
+                blue_triangle(ferris).await.expect("Failed running auto");
+            }
         }
     }
 }
+
+pub async fn blue_triangle(robot: Ferris) -> Result<(), Box<dyn std::error::Error>> {
+    println!("RUNNING");
+
+    let mut drivetrain = robot.drivetrain.deref().borrow_mut();
+
+    drivetrain
+        .odometry
+        .set_abs(Vector2::new(8.075126647949219, 2.0993127822875977));
+
+    drive("BlueTriangle", &mut drivetrain, 1).await?;
+    println!("BlueTriangle.1 done");
+
+    sleep(Duration::from_secs_f64(1.)).await;
+
+    drive("BlueTriangle", &mut drivetrain, 2).await?;
+    println!("BlueTriangle.2 done");
+
+    sleep(Duration::from_secs_f64(1.)).await;
+
+    drive("BlueTriangle", &mut drivetrain, 3).await?;
+    println!("BlueTriangle.3 done");
+
+    Ok(())
+}

src/auto/path.rs

@@ -1 +1,132 @@
+use std::time::Duration;
+use tokio::fs::File;
 
+use nalgebra::Vector2;
+use tokio::io::AsyncReadExt;
+use tokio::time::{sleep, Instant};
+use uom::si::{
+    angle::radian,
+    f64::{Length, Time},
+    length::{foot, meter},
+    time::{millisecond, second},
+    velocity::meter_per_second,
+};
+use wpi_trajectory::Path;
+
+use crate::subsystems::SwerveControlStyle;
+use crate::{
+    constants::drivetrain::{
+        SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KF, SWERVE_DRIVE_KFA, SWERVE_DRIVE_KI,
+        SWERVE_DRIVE_KP, SWERVE_DRIVE_MAX_ERR, SWERVE_TURN_KP,
+    },
+    subsystems::Drivetrain,
+};
+
+// TODO: Test
+pub async fn drive(
+    name: &str,
+    drivetrain: &mut Drivetrain,
+    waypoint_index: usize,
+) -> Result<(), Box<dyn std::error::Error>> {
+    let mut path_content = String::new();
+    File::open(format!("/home/lvuser/deploy/choreo/{}.traj", name))
+        .await?
+        .read_to_string(&mut path_content)
+        .await?;
+
+    let path = Path::from_trajectory(&path_content)?;
+    let waypoints = path.waypoints();
+
+    if waypoint_index >= waypoints.len() {
+        return Err("Waypoint index out of bounds".into());
+    }
+
+    // Get start and end times for the waypoint
+    let start_time = if waypoint_index == 0 {
+        0.0
+    } else {
+        waypoints[waypoint_index - 1]
+    };
+    let end_time = waypoints[waypoint_index];
+
+    // Follow path for this segment
+    follow_path_segment(drivetrain, path, 0., 3.4).await;
+    drivetrain.set_speeds(0., 0., 0., SwerveControlStyle::FieldOriented);
+    Ok(())
+}
+
+pub async fn follow_path_segment(
+    drivetrain: &mut Drivetrain,
+    path: Path,
+    start_time: f64,
+    end_time: f64,
+) {
+    let start = Instant::now();
+    let mut last_error = Vector2::zeros();
+    let mut last_loop = Instant::now();
+    let mut i = Vector2::zeros();
+
+    loop {
+        let now = Instant::now();
+        let dt = now - last_loop;
+        last_loop = now;
+
+        //println!("x: {}, y: {}", drivetrain.odometry.position.x, drivetrain.odometry.position.y);
+
+        let elapsed = start.elapsed().as_secs_f64() + start_time;
+
+        // Exit if we've reached the end time for this segment
+        if elapsed > end_time {
+            break;
+        }
+
+        let setpoint = path.get(Time::new::<second>(elapsed));
+
+        let angle = -setpoint.heading;
+        let position = Vector2::new(setpoint.x.get::<meter>(), setpoint.y.get::<meter>());
+
+        let mut error_position = position - drivetrain.odometry.position;
+        let mut error_angle = (angle - drivetrain.get_angle()).get::<radian>();
+
+        if error_position.abs().max() < SWERVE_DRIVE_IE {
+            i += error_position;
+        }
+
+        if elapsed > path.length().get::<second>()
+            && error_position.abs().max() < SWERVE_DRIVE_MAX_ERR
+            && error_angle.abs() < 0.075
+        {
+            break;
+        }
+
+        error_angle *= SWERVE_TURN_KP;
+        error_position *= -SWERVE_DRIVE_KP;
+
+        let mut speed = error_position;
+
+        let velocity = Vector2::new(setpoint.velocity_x, setpoint.velocity_y);
+        let velocity = velocity.map(|x| x.get::<meter_per_second>());
+
+        let velocity_next = Vector2::new(setpoint.velocity_x, setpoint.velocity_y)
+            .map(|x| x.get::<meter_per_second>());
+
+        let acceleration = (velocity_next - velocity) * 1000. / 20.;
+
+        speed += velocity * -SWERVE_DRIVE_KF;
+        speed += acceleration * -SWERVE_DRIVE_KFA;
+        speed += i * -SWERVE_DRIVE_KI * dt.as_secs_f64() * 9.;
+
+        let speed_s = speed;
+        speed += (speed - last_error) * -SWERVE_DRIVE_KD * dt.as_secs_f64() * 9.;
+        last_error = speed_s;
+
+        drivetrain.set_speeds(
+            speed.x,
+            speed.y,
+            error_angle,
+            SwerveControlStyle::FieldOriented,
+        );
+
+        sleep(Duration::from_millis(20)).await;
+    }
+}