code cleanup.

This commit is contained in:
Gabe Venberg 2023-10-27 11:46:03 -05:00
parent 3963c0fc90
commit 0b363ed8e7
4 changed files with 17 additions and 10 deletions

View file

@ -59,7 +59,7 @@ where
E: Debug, E: Debug,
{ {
let data = get_data(sensor, display, timer); let data = get_data(sensor, display, timer);
return calibrate(&data); calibrate(&data)
} }
fn get_data<I, T, E>( fn get_data<I, T, E>(
@ -124,7 +124,7 @@ where
} }
display.show(timer, leds, 200); display.show(timer, leds, 200);
} }
return data; data
} }
fn difference_square(a: Measurement, b: Measurement) -> f32 { fn difference_square(a: Measurement, b: Measurement) -> f32 {
@ -164,9 +164,9 @@ fn calibrate(data: &[Measurement]) -> Calibration {
center.z += point.z; center.z += point.z;
} }
center.x = center.x / data.len() as i32; center.x /= data.len() as i32;
center.y = center.y / data.len() as i32; center.y /= data.len() as i32;
center.z = center.z / data.len() as i32; center.z /= data.len() as i32;
let mut current = center; let mut current = center;
let mut score = measure_score(current, data); let mut score = measure_score(current, data);

View file

@ -13,6 +13,7 @@ pub enum Direction {
NorthWest, NorthWest,
} }
//forward is towards usb port
const NORTH: [[u8; 5]; 5] = [ const NORTH: [[u8; 5]; 5] = [
[0, 0, 1, 0, 0], [0, 0, 1, 0, 0],
[0, 1, 1, 1, 0], [0, 1, 1, 1, 0],

View file

@ -1,6 +1,6 @@
use core::mem::swap; use core::mem::swap;
pub struct Point { struct Point {
x: isize, x: isize,
y: isize, y: isize,
} }
@ -9,7 +9,7 @@ impl Point {
/// converts a point (representing a point on a 4 quadrant grid) into a upoint (representing a /// converts a point (representing a point on a 4 quadrant grid) into a upoint (representing a
/// point on a 1 quadrant grid with the origin in the bottom-left corner). Returns none if /// point on a 1 quadrant grid with the origin in the bottom-left corner). Returns none if
/// the resulting point would have either number negative. /// the resulting point would have either number negative.
pub fn to_upoint(&self, zero_coord: &UPoint) -> Option<UPoint> { fn to_upoint(&self, zero_coord: &UPoint) -> Option<UPoint> {
Some(UPoint { Some(UPoint {
x: zero_coord.x.checked_add_signed(self.x)?, x: zero_coord.x.checked_add_signed(self.x)?,
y: zero_coord.y.checked_add_signed(self.y)?, y: zero_coord.y.checked_add_signed(self.y)?,
@ -17,7 +17,7 @@ impl Point {
} }
} }
pub struct UPoint { struct UPoint {
x: usize, x: usize,
y: usize, y: usize,
} }
@ -33,7 +33,7 @@ impl UPoint {
} }
} }
pub fn draw_line<const X: usize, const Y: usize>( fn draw_line<const X: usize, const Y: usize>(
mut p0: Point, mut p0: Point,
mut p1: Point, mut p1: Point,
matrix: &mut [[u8; X]; Y], matrix: &mut [[u8; X]; Y],
@ -70,3 +70,7 @@ pub fn draw_line<const X: usize, const Y: usize>(
} }
} }
} }
fn heading_to_sector(sectors: u8, heading: f32) -> u8 {
todo!()
}

View file

@ -14,7 +14,8 @@ pub struct NedMeasurement {
pub z: f32, pub z: f32,
} }
//theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west //theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west (desired)
//theta=0 at south, pi/-pi at north, pi/2 at east, and -pi/2 at west (current)
pub struct Heading(pub f32); pub struct Heading(pub f32);
/// board has forward in the -y direction and right in the -x direction, and down in the -z. (SWU), algs for tilt compensation /// board has forward in the -y direction and right in the -x direction, and down in the -z. (SWU), algs for tilt compensation
@ -57,6 +58,7 @@ pub fn calc_tilt_calibrated_measurement(
} }
} }
//0 is the top sector and positive is clockwise, negative is counterclockwise.
pub fn heading_from_measurement(measurement: NedMeasurement) -> Heading { pub fn heading_from_measurement(measurement: NedMeasurement) -> Heading {
Heading(atan2f(-measurement.y, measurement.x)) Heading(atan2f(-measurement.y, measurement.x))
} }