From 4e4b314ccd7b42168ce23a7356e2c32ff19e1094 Mon Sep 17 00:00:00 2001 From: Gabe Venberg Date: Sun, 29 Oct 2023 20:27:26 -0500 Subject: [PATCH] hooked up the line drawing algorithm to the compass. This entailed fixing a few bugs in it as well. --- hardware_main/src/main.rs | 20 ++- independent_logic/src/heading_drawing.rs | 57 ++---- independent_logic/src/led.rs | 133 -------------- independent_logic/src/lib.rs | 4 +- independent_logic/src/line_drawing.rs | 216 ++++++++++++++++++++++- 5 files changed, 240 insertions(+), 190 deletions(-) delete mode 100644 independent_logic/src/led.rs diff --git a/hardware_main/src/main.rs b/hardware_main/src/main.rs index 7d8fe31..e2ae672 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -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) } } diff --git a/independent_logic/src/heading_drawing.rs b/independent_logic/src/heading_drawing.rs index d54cbe0..731b038 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -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( heading: f32, -) -> FourQuadrantMatrix<{ X }, { Y }, u8> { - let mut ret = FourQuadrantMatrix::new(UPoint { x: X / 2, y: Y / 2 }); - draw_line::(&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::(&heading_to_line(heading, X.min(Y)), matrix); } diff --git a/independent_logic/src/led.rs b/independent_logic/src/led.rs deleted file mode 100644 index f1a7c83..0000000 --- a/independent_logic/src/led.rs +++ /dev/null @@ -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 - } -} diff --git a/independent_logic/src/lib.rs b/independent_logic/src/lib.rs index 24d4daa..5dc07dc 100644 --- a/independent_logic/src/lib.rs +++ b/independent_logic/src/lib.rs @@ -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; diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs index 1afa770..9847782 100644 --- a/independent_logic/src/line_drawing.rs +++ b/independent_logic/src/line_drawing.rs @@ -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 { matrix: [[T; X]; Y], - pub zero_coord: UPoint, + max_point: Point, + min_point: Point, + zero_coord: UPoint, } impl 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 IndexMut 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( 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!( + 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!( + 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!( + 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!( + 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!( + 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!( + 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> =