hooked up the line drawing algorithm to the compass.

This entailed fixing a few bugs in it as well.
This commit is contained in:
Gabe Venberg 2023-10-29 20:27:26 -05:00
parent 3e29d8bc6a
commit 4e4b314ccd
5 changed files with 240 additions and 190 deletions

View file

@ -6,6 +6,7 @@ use core::f32::consts::PI;
use calibration::Calibration;
use cortex_m_rt::entry;
use independent_logic::line_drawing::{FourQuadrantMatrix, UPoint};
use lsm303agr::interface::I2cInterface;
use lsm303agr::mode::MagContinuous;
use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate, Measurement};
@ -27,7 +28,7 @@ use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A};
use crate::calibration::calc_calibration;
use independent_logic::{
led::{direction_to_led, theta_to_direction},
heading_drawing::draw_heading,
tilt_compensation::{
calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading,
NedMeasurement,
@ -74,10 +75,14 @@ fn main() -> ! {
let mut calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
#[cfg(not(feature = "calibration"))]
let mut calibration = calibration::Calibration::default();
let mut current_display: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
rprintln!("Calibration: {:?}", calibration);
let mut tilt_correction_enabled: bool = true;
// let mut heading = Heading(0.0);
loop {
if channel_button_b.is_event_triggered() {
calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
@ -90,12 +95,15 @@ fn main() -> ! {
channel_button_a.reset_events()
}
current_display.reset_matrix();
// heading.0 = (heading.0+PI/16.0)%(2.0*PI);
// rprintln!("heading is {}PI", heading.0/PI);
let heading = calc_heading(&mut sensor, &calibration, &tilt_correction_enabled);
display.show(
&mut timer,
direction_to_led(theta_to_direction(heading)),
DELAY,
)
draw_heading::<5, 5>(heading.0, &mut current_display);
rprintln!("finished drawing");
display.show(&mut timer, current_display.into(), DELAY)
}
}

View file

@ -1,53 +1,20 @@
#![allow(unused)]
use core::f32::consts::PI;
use libm::{cosf, roundf, sinf};
use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct Sector {
total_sectors: usize,
sector: usize,
}
//heading starts at north, with the positive direction being clockwise.
//Heading ranges from -pi to pi.
//
//sectors have 0 at north an proceed clockwise, always being positive.
fn heading_to_sector(sectors: usize, heading: f32) -> Sector {
let half_sector = PI / sectors as f32;
let sector_size = 2.0 * half_sector;
Sector {
total_sectors: sectors,
sector: (modulo(heading + half_sector, 2.0 * PI) / (sector_size)) as usize,
}
}
fn modulo(a: f32, b: f32) -> f32 {
((a % b) + b) % b
}
use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point};
fn heading_to_line(heading: f32, square_size: usize) -> Line {
todo!()
Line(
Point { x: 0, y: 0 },
Point {
x: roundf((square_size as f32) * sinf(heading)) as isize,
y: roundf((square_size as f32) * cosf(heading)) as isize,
},
)
}
pub fn draw_heading<const X: usize, const Y: usize>(
heading: f32,
) -> FourQuadrantMatrix<{ X }, { Y }, u8> {
let mut ret = FourQuadrantMatrix::new(UPoint { x: X / 2, y: Y / 2 });
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), &mut ret);
ret
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn sectors() {
assert_eq!(heading_to_sector(4,0.0).sector, 0);
assert_eq!(heading_to_sector(4,PI/2.0).sector, 1);
assert_eq!(heading_to_sector(4,-PI/2.0).sector, 3);
assert_eq!(heading_to_sector(4,PI).sector, 2);
assert_eq!(heading_to_sector(4,-PI).sector, 2);
}
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
) {
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
}

View file

@ -1,133 +0,0 @@
use core::f32::consts::PI;
use crate::tilt_compensation::Heading;
#[derive(Debug)]
pub enum Direction {
North,
NorthEast,
East,
SouthEast,
South,
SouthWest,
West,
NorthWest,
}
//forward is towards usb port
const NORTH: [[u8; 5]; 5] = [
[0, 0, 1, 0, 0],
[0, 1, 1, 1, 0],
[1, 0, 1, 0, 1],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
];
const NORTH_EAST: [[u8; 5]; 5] = [
[1, 1, 1, 0, 0],
[1, 1, 0, 0, 0],
[1, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[0, 0, 0, 0, 1],
];
const EAST: [[u8; 5]; 5] = [
[0, 0, 1, 0, 0],
[0, 1, 0, 0, 0],
[1, 1, 1, 1, 1],
[0, 1, 0, 0, 0],
[0, 0, 1, 0, 0],
];
const SOUTH_EAST: [[u8; 5]; 5] = [
[0, 0, 0, 0, 1],
[0, 0, 0, 1, 0],
[1, 0, 1, 0, 0],
[1, 1, 0, 0, 0],
[1, 1, 1, 0, 0],
];
const SOUTH: [[u8; 5]; 5] = [
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[1, 0, 1, 0, 1],
[0, 1, 1, 1, 0],
[0, 0, 1, 0, 0],
];
const SOUTH_WEST: [[u8; 5]; 5] = [
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[0, 0, 1, 0, 1],
[0, 0, 0, 1, 1],
[0, 0, 1, 1, 1],
];
const WEST: [[u8; 5]; 5] = [
[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[1, 1, 1, 1, 1],
[0, 0, 0, 1, 0],
[0, 0, 1, 0, 0],
];
const NORTH_WEST: [[u8; 5]; 5] = [
[0, 0, 1, 1, 1],
[0, 0, 0, 1, 1],
[0, 0, 1, 0, 1],
[0, 1, 0, 0, 0],
[1, 0, 0, 0, 0],
];
pub fn direction_to_led(direction: Direction) -> [[u8; 5]; 5] {
match direction {
Direction::North => NORTH,
Direction::NorthEast => NORTH_EAST,
Direction::East => EAST,
Direction::SouthEast => SOUTH_EAST,
Direction::South => SOUTH,
Direction::SouthWest => SOUTH_WEST,
Direction::West => WEST,
Direction::NorthWest => NORTH_WEST,
}
}
pub fn theta_to_direction(heading: Heading) -> Direction {
// if heading.0 < (-7. * PI / 8.) {
// Direction::North
// } else if heading.0 < (-5. * PI / 8.) {
// Direction::NorthWest
// } else if heading.0 < (-3. * PI / 8.) {
// Direction::West
// } else if heading.0 < (-PI / 8.) {
// Direction::SouthWest
// } else if heading.0 < (PI / 8.) {
// Direction::South
// } else if heading.0 < (3. * PI / 8.) {
// Direction::SouthEast
// } else if heading.0 < (5. * PI / 8.) {
// Direction::East
// } else if heading.0 < (7. * PI / 8.) {
// Direction::NorthEast
// } else {
// Direction::North
// }
if heading.0 < (-7. * PI / 8.) {
Direction::South
} else if heading.0 < (-5. * PI / 8.) {
Direction::SouthEast
} else if heading.0 < (-3. * PI / 8.) {
Direction::East
} else if heading.0 < (-PI / 8.) {
Direction::NorthEast
} else if heading.0 < (PI / 8.) {
Direction::North
} else if heading.0 < (3. * PI / 8.) {
Direction::NorthWest
} else if heading.0 < (5. * PI / 8.) {
Direction::West
} else if heading.0 < (7. * PI / 8.) {
Direction::SouthWest
} else {
Direction::South
}
}

View file

@ -1,5 +1,5 @@
#![no_std]
//to help debug failed tests wiht dbg!()
#![cfg_attr(not(test), no_std)]
pub mod heading_drawing;
pub mod line_drawing;
pub mod tilt_compensation;
pub mod led;

View file

@ -2,6 +2,8 @@ use core::{
mem::swap,
ops::{Index, IndexMut},
};
#[cfg(test)]
use std::dbg;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Point {
@ -44,7 +46,9 @@ impl UPoint {
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
matrix: [[T; X]; Y],
pub zero_coord: UPoint,
max_point: Point,
min_point: Point,
zero_coord: UPoint,
}
impl<const X: usize, const Y: usize, T> FourQuadrantMatrix<{ X }, { Y }, T>
@ -55,9 +59,51 @@ where
pub fn new(zero_coord: UPoint) -> FourQuadrantMatrix<{ X }, { Y }, T> {
FourQuadrantMatrix {
matrix: [[T::default(); X]; Y],
max_point: UPoint { x: X - 1, y: 0 }.to_point(&zero_coord),
min_point: UPoint { x: 0, y: Y - 1 }.to_point(&zero_coord),
zero_coord,
}
}
pub fn zero_coord(&self) -> UPoint {
self.zero_coord
}
pub fn min_point(&self) -> Point {
self.min_point
}
pub fn max_point(&self) -> Point {
self.max_point
}
pub fn bound_point(&self, point: &mut Point) {
if point.x > self.max_point.x {
point.x = self.max_point.x
}
if point.y > self.max_point.y {
point.y = self.max_point.y
}
if point.x < self.min_point.x {
point.x = self.min_point.x
}
if point.y < self.min_point.y {
point.y = self.min_point.y
}
}
pub fn is_in_bounds(&self, point: &Point) -> bool {
point.x <= self.max_point.x
&& point.y <= self.max_point.y
&& point.x >= self.min_point.x
&& point.y >= self.min_point.y
}
pub fn reset_matrix(&mut self) {
self.matrix = [[T::default(); X]; Y];
}
}
impl<T, const X: usize, const Y: usize> IndexMut<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
@ -94,48 +140,90 @@ pub struct Line(pub Point, pub Point);
pub struct ULine(pub UPoint, pub UPoint);
/// Renders a line into a matrix of pixels.
/// Will not attempt to mutate outside bounds of the matrix, so it is safe to draw lines that
/// extend past its edges.
pub fn draw_line<const X: usize, const Y: usize>(
line: &Line,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
) {
let mut line = *line;
let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.x).abs();
#[cfg(test)]
dbg!(line);
// Is it steeper than 45°? If so, we transpose the line. This essentially guarantees we are
// drawing a line less steep than 45°.
#[cfg(test)]
dbg!((line.0.x - line.1.x).abs() < (line.0.y - line.1.y).abs());
let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.y).abs();
if steep {
swap(&mut line.0.x, &mut line.0.y);
swap(&mut line.1.x, &mut line.1.y);
}
// If our line is running right-to-left, flip the points
// so we start on the left.
if line.0.x > line.1.x {
swap(&mut line.0.x, &mut line.1.x);
swap(&mut line.0.y, &mut line.1.y)
}
#[cfg(test)]
dbg!((line, steep));
let dx = line.1.x - line.0.x;
let dy = line.1.y - line.0.y;
let derror2 = dy.abs() * 2;
let mut error2 = 0;
let mut y = line.0.y;
let mut draw_point: Point;
//if the first point is out of bounds, we want to wait for us to get in bounds before arming
//the early return.
let mut prev_out_of_bounds = !matrix.is_in_bounds(&line.0);
// For each X coordinate (which is actually a Y coordinate if the line is steep), we calculate
// the Y coordinate the same way as before
for x in line.0.x..=line.1.x {
#[cfg(test)]
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
if steep {
matrix[Point { x: y, y: x }] = 1;
// Remember the transpose? This is where we undo it, by swapping our y and x
// coordinates again
draw_point = Point { x: y, y: x };
} else {
matrix[Point { x, y }] = 1;
draw_point = Point { x, y };
}
#[cfg(test)]
dbg!(draw_point);
if matrix.is_in_bounds(&draw_point) {
matrix[draw_point] = 1;
prev_out_of_bounds = false;
} else {
if !prev_out_of_bounds {
break;
}
prev_out_of_bounds = true;
}
error2 += derror2;
#[cfg(test)]
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
if error2 > dx {
y += if line.1.y > line.0.y { 1 } else { -1 };
error2 -= dx * 2
}
#[cfg(test)]
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn point_upoint_conv() {
let zero_coord = UPoint { x: 2, y: 2 };
@ -223,6 +311,126 @@ mod tests {
)
}
#[test]
fn diagonal_signed_both_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 0, 0, 1],
[0, 0, 0, 1, 0],
[0, 0, 1, 0, 0],
[0, 1, 0, 0, 0],
[1, 0, 0, 0, 0],
]
);
}
#[test]
fn diagonal_signed_first_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 0, 0, 1],
[0, 0, 0, 1, 0],
[0, 0, 1, 0, 0],
[0, 1, 0, 0, 0],
[1, 0, 0, 0, 0],
]
);
}
#[test]
fn diagonal_signed_second_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 0, 0, 1],
[0, 0, 0, 1, 0],
[0, 0, 1, 0, 0],
[0, 1, 0, 0, 0],
[1, 0, 0, 0, 0],
]
);
}
#[test]
fn vertical_signed_both_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
]
);
}
#[test]
fn vertical_signed_first_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
]
);
}
#[test]
fn vertical_signed_second_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
]
);
}
#[test]
fn cross_signed_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =