code cleanup.
This commit is contained in:
parent
3963c0fc90
commit
0b363ed8e7
|
@ -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);
|
||||||
|
|
|
@ -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],
|
||||||
|
|
|
@ -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!()
|
||||||
|
}
|
||||||
|
|
|
@ -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))
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue