Merge pull request #118 from RIT-MDRC/mike/encoder-angles

Mike/encoder angles

core_pb/Cargo.toml

@@ -53,4 +53,7 @@ wasm-bindgen-futures = "0.4.42"
 web-time = "1.1.0"
 
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
-async-tungstenite = { version = "0.26.2", optional = true, features = ["async-std-runtime"] }
+async-tungstenite = { version = "0.26.2", optional = true, features = ["async-std-runtime"] }
+
+[build-dependencies]
+nalgebra = { version = "0.32.5", default-features = false }

core_pb/build.rs

@@ -0,0 +1,299 @@
+use nalgebra::{Point2, Vector2};
+use std::env;
+use std::fs::File;
+use std::io::{self, Write};
+use std::path::Path;
+
+const GRID_SIZE: usize = 32;
+type Grid = [[bool; GRID_SIZE]; GRID_SIZE];
+
+fn main() -> io::Result<()> {
+    // Generate regions for localization
+    // Define the output path for the generated file
+    let out_dir = env::var("OUT_DIR").unwrap();
+    let dest_path = Path::new(&out_dir).join("generated_grids.rs");
+
+    // List standard grids
+    let manifest_dir = env::var("CARGO_MANIFEST_DIR").expect("CARGO_MANIFEST_DIR not set");
+    let grids_path = Path::new(&manifest_dir)
+        .join("src")
+        .join("grid")
+        .join("grids");
+    let mut grids: Vec<(String, Grid)> = vec![];
+    for path in grids_path.read_dir()?.flatten() {
+        if let Some(file_name) = path.file_name().to_str() {
+            let grid_text = String::from_utf8(std::fs::read(grids_path.join(file_name))?).unwrap();
+            grids.push((
+                file_name.to_string().split('.').next().unwrap().to_string(),
+                grid_text
+                    .trim_ascii()
+                    .split("\n")
+                    .map(|x| {
+                        x.trim_ascii()
+                            .chars()
+                            .map(|c| c == 'W')
+                            .collect::<Vec<_>>()
+                            .try_into()
+                            .expect("Incorrect grid width!")
+                    })
+                    .collect::<Vec<_>>()
+                    .try_into()
+                    .expect("Incorrect grid height!"),
+            ));
+        }
+    }
+
+    let mut f = File::create(&dest_path)?;
+
+    write!(f, "// This file was generated by build.rs\n\npub const T: bool = true;\npub const F: bool = false;\n\n")?;
+    for (name, grid) in &grids {
+        let grid_str = "[".to_string()
+            + &grid
+                .into_iter()
+                .map(|row| {
+                    "\n\t[".to_string()
+                        + &row
+                            .into_iter()
+                            .map(|c| if *c { "T, " } else { "F, " })
+                            .collect::<String>()
+                        + "],"
+                })
+                .collect::<String>()
+            + "\n];\n";
+        write!(
+            f,
+            "pub const GENERATED_GRID_{}: [[bool; {GRID_SIZE}]; {GRID_SIZE}] = {grid_str}\n",
+            name.to_uppercase(),
+        )?;
+    }
+
+    write!(f, "\nuse crate::region_localization::Region;\n\n")?;
+    write!(
+        f,
+        "pub fn get_grid_regions(grid: StandardGrid) -> &'static [Region] {{\n\tmatch grid {{"
+    )?;
+    for (name, _grid) in &grids {
+        write!(
+            f,
+            "\n\t\tStandardGrid::{} => &GENERATED_REGIONS_{},",
+            to_camel_case(name),
+            name.to_uppercase()
+        )?;
+    }
+    write!(f, "\n\t}}\n}}\n")?;
+    for (name, grid) in &grids {
+        let regions = get_all_regions(*grid);
+        let region_str = regions
+            .into_iter()
+            .map(|(_, r)| format!(
+                "\n\tRegion {{\n\t\tlow_xy: Point2::new({}, {}),\n\t\thigh_xy: Point2::new({}, {}),\n\n\t\tdist_low_xy_to_wall: {:?},\n\t}},",
+                r.low_xy.x, r.low_xy.y, r.high_xy.x, r.high_xy.y, r.dist_low_xy_to_wall
+            ))
+            .collect::<String>();
+        write!(
+            f,
+            "\npub const GENERATED_REGIONS_{}: &'static [Region] = &[{region_str}\n];\n",
+            name.to_uppercase()
+        )?;
+    }
+
+    Ok(())
+}
+
+fn to_camel_case(s: &str) -> String {
+    let mut camel = String::new();
+    let mut capitalize_next = true;
+
+    for c in s.chars() {
+        if c.is_alphanumeric() {
+            if capitalize_next {
+                camel.extend(c.to_uppercase());
+                capitalize_next = false;
+            } else {
+                camel.extend(c.to_lowercase());
+            }
+        } else {
+            capitalize_next = true;
+        }
+    }
+
+    camel
+}
+
+const VECTORS: [Vector2<i8>; 4] = [
+    Vector2::new(1, 0),  // right
+    Vector2::new(0, 1),  // up
+    Vector2::new(-1, 0), // left
+    Vector2::new(0, -1), // down
+];
+
+#[derive(Copy, Clone, Debug, PartialOrd, PartialEq)]
+pub struct Region {
+    pub low_xy: Point2<i8>,
+    pub high_xy: Point2<i8>,
+
+    pub dist_low_xy_to_wall: [i8; 4],
+}
+
+fn get_at(grid: Grid, at: Vector2<i8>) -> bool {
+    if at.x < 0 || at.y < 0 || at.x as usize >= grid.len() || at.y as usize >= grid[0].len() {
+        false
+    } else {
+        grid[at.x as usize][at.y as usize]
+    }
+}
+
+fn v_to_p(v: Vector2<i8>) -> Point2<i8> {
+    Point2::new(v.x, v.y)
+}
+
+#[derive(Copy, Clone, Debug, PartialOrd, PartialEq)]
+enum PointType {
+    Wall,
+    VerticalBoundary(bool),
+    HorizontalBoundary(bool),
+}
+
+fn is_special(boundary: Option<PointType>) -> bool {
+    match boundary {
+        Some(PointType::Wall) => true,
+        None => false,
+        Some(PointType::HorizontalBoundary(b)) | Some(PointType::VerticalBoundary(b)) => !b,
+    }
+}
+
+const ONE_X: Vector2<i8> = Vector2::new(1, 0);
+const ONE_Y: Vector2<i8> = Vector2::new(0, 1);
+
+fn get_boundary(grid: Grid, p: Vector2<i8>) -> Option<PointType> {
+    // If the point is a wall, returns None
+    if get_at(grid, p) {
+        Some(PointType::Wall)
+    }
+    // If the point lies on a vertical region boundary
+    else if (get_at(grid, p - ONE_Y) && !get_at(grid, p - ONE_Y - ONE_X))
+        || (get_at(grid, p + ONE_Y) && !get_at(grid, p + ONE_Y - ONE_X))
+    {
+        Some(PointType::VerticalBoundary(true))
+    } else if (get_at(grid, p - ONE_Y) && !get_at(grid, p - ONE_Y + ONE_X))
+        || (get_at(grid, p + ONE_Y) && !get_at(grid, p + ONE_Y + ONE_X))
+    {
+        Some(PointType::HorizontalBoundary(false))
+    }
+    // If the point lies on a horizontal region boundary
+    else if (get_at(grid, p - ONE_X) && !get_at(grid, p - ONE_X - ONE_Y))
+        || (get_at(grid, p + ONE_X) && !get_at(grid, p + ONE_X - ONE_Y))
+    {
+        Some(PointType::HorizontalBoundary(true))
+    } else if (get_at(grid, p - ONE_X) && !get_at(grid, p - ONE_X + ONE_Y))
+        || (get_at(grid, p + ONE_X) && !get_at(grid, p + ONE_X + ONE_Y))
+    {
+        Some(PointType::HorizontalBoundary(false))
+    } else {
+        None
+    }
+}
+
+fn build_horizontal_region(grid: Grid, p: Vector2<i8>) -> Region {
+    let mut end = p + ONE_X;
+    while get_boundary(grid, end).is_none() {
+        end += ONE_X;
+    }
+    Region {
+        low_xy: v_to_p(p - ONE_Y),
+        high_xy: v_to_p(end + ONE_Y),
+
+        dist_low_xy_to_wall: [
+            1 + get_empty_for(grid, p + ONE_X, VECTORS[0]),
+            2,
+            get_empty_for(grid, p, VECTORS[2]),
+            0,
+        ],
+    }
+}
+
+fn build_vertical_region(grid: Grid, p: Vector2<i8>) -> Region {
+    let mut end = p + ONE_Y;
+    while get_boundary(grid, end).is_none() {
+        end += ONE_Y;
+    }
+    Region {
+        low_xy: v_to_p(p - ONE_X),
+        high_xy: v_to_p(end + ONE_X),
+
+        dist_low_xy_to_wall: [
+            2,
+            1 + get_empty_for(grid, p + ONE_Y, VECTORS[1]),
+            0,
+            get_empty_for(grid, p, VECTORS[3]),
+        ],
+    }
+}
+
+fn get_empty_for(grid: Grid, mut at: Vector2<i8>, dir: Vector2<i8>) -> i8 {
+    let mut count = 0;
+    while !get_at(grid, at) {
+        at += dir;
+        count += 1;
+    }
+    count
+}
+
+/// Looks at the given point and returns up to 1 region
+///
+/// - If the point is a wall, returns None
+/// - If the point lies entirely at the bottom left (-x,-y) of a region bounded below (-y) and to
+/// the left (-x) by walls, returns the corresponding region
+/// - If the point lies on a vertical region boundary, where the n-wide 2-tall region
+/// lies to the right (+x), returns the corresponding region
+/// - If the point lies on a horizontal region boundary, where the 2-wide n-tall region
+/// lies above (+y), returns the corresponding region
+pub fn get_region_for_unique_p(grid: Grid, at: Point2<i8>) -> Option<Region> {
+    let p = Vector2::new(at.x, at.y);
+    match get_boundary(grid, p) {
+        Some(PointType::Wall)
+        | Some(PointType::VerticalBoundary(false))
+        | Some(PointType::HorizontalBoundary(false)) => None,
+        None => {
+            if is_special(get_boundary(grid, p - ONE_X))
+                && is_special(get_boundary(grid, p - ONE_Y))
+            {
+                if get_boundary(grid, p + ONE_X).is_none() {
+                    Some(build_horizontal_region(grid, p - ONE_X))
+                } else if get_boundary(grid, p + ONE_Y).is_none() {
+                    Some(build_vertical_region(grid, p - ONE_Y))
+                } else {
+                    // 2x2 region
+                    // Some(build_horizontal_region(grid, p - ONE_X))
+                    Some(Region {
+                        low_xy: v_to_p(p - ONE_Y - ONE_X),
+                        high_xy: v_to_p(p + ONE_Y + ONE_X),
+
+                        dist_low_xy_to_wall: [
+                            1 + get_empty_for(grid, p, VECTORS[0]),
+                            1 + get_empty_for(grid, p, VECTORS[1]),
+                            get_empty_for(grid, p - ONE_X, VECTORS[2]),
+                            get_empty_for(grid, p - ONE_Y, VECTORS[3]),
+                        ],
+                    })
+                }
+            } else {
+                None
+            }
+        }
+        Some(PointType::VerticalBoundary(true)) => Some(build_horizontal_region(grid, p)),
+        Some(PointType::HorizontalBoundary(true)) => Some(build_vertical_region(grid, p)),
+    }
+}
+
+pub fn get_all_regions(grid: Grid) -> Vec<(Point2<i8>, Region)> {
+    (0..GRID_SIZE)
+        .flat_map(|x| {
+            (0..GRID_SIZE).map(move |y| {
+                get_region_for_unique_p(grid, Point2::new(x as i8, y as i8))
+                    .map(|r| (Point2::new(x as i8, y as i8), r))
+            })
+        })
+        .flatten()
+        .collect()
+}

core_pb/src/drive_system.rs

@@ -166,6 +166,38 @@ impl DriveSystem<3> {
             }
         }
     }
+
+    /// Given signed motor speeds, find the angular velocity of the robot
+    ///
+    /// # Arguments
+    ///
+    /// - motor_speeds: the signed speeds of the motors, in rad/s
+    pub fn get_actual_rotational_vel_omni(&self, motor_speeds: [f32; 3]) -> f32 {
+        match self {
+            DriveSystem::Omniwheel {
+                wheel_radius,
+                robot_radius,
+                forwards_is_clockwise,
+                ..
+            } => {
+                // rotational to linear
+                let rot_to_lin = |v: f32, fic: bool| {
+                    if fic {
+                        v * *wheel_radius
+                    } else {
+                        -v * *wheel_radius
+                    }
+                };
+                let v_a = rot_to_lin(motor_speeds[0], forwards_is_clockwise[0]);
+                let v_b = rot_to_lin(motor_speeds[1], forwards_is_clockwise[1]);
+                let v_c = rot_to_lin(motor_speeds[2], forwards_is_clockwise[2]);
+
+                let w = (v_a + v_b + v_c) / 3.0;
+
+                w / robot_radius
+            }
+        }
+    }
 }
 
 #[cfg(test)]

core_pb/src/driving/motors.rs

@@ -68,7 +68,7 @@ pub async fn motors_task<T: RobotMotorsBehavior, M: RobotTaskMessenger>(
     });
 
     // TODO: make this a tunable param
-    let angle_p = 6.0;
+    let angle_p = 2.0;
     let angle_tol = 0.03; // rad
 
     let drive_system = robot.drive_system;
@@ -128,7 +128,7 @@ pub async fn motors_task<T: RobotMotorsBehavior, M: RobotTaskMessenger>(
                 if let Ok(angle) = sensors.angle {
                     target_velocity.1 = adjust_ang_vel(angle, 0.0, angle_p, angle_tol);
                     let angle = Rotation2::new(angle).angle();
-                    if angle.abs() < 5.0_f32.to_radians() {
+                    if angle.abs() < 20.0_f32.to_radians() {
                         // now that we've made sure we're facing the right way, try to follow the path
                         if let Some(vel) = pure_pursuit(
                             sensors,