hooked up the line drawing algorithm to the compass.

This entailed fixing a few bugs in it as well.

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(
+        draw_heading::<5, 5>(heading.0, &mut current_display);
-            &mut timer,
+        rprintln!("finished drawing");
-            direction_to_led(theta_to_direction(heading)),
+        display.show(&mut timer, current_display.into(), DELAY)
-            DELAY,
-        )
     }
 }
 

independent_logic/src/heading_drawing.rs

@@ -1,53 +1,20 @@
-#![allow(unused)]
+use libm::{cosf, roundf, sinf};
-use core::f32::consts::PI;
 
-use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
+use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point};
-
-#[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
-}
 
 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> {
+    matrix: &mut 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);
+    draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
-    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);
-    }
 }

independent_logic/src/led.rs

@@ -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
-    }
-}

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;

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<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> =