added ability to traverse a segment in reverse

src/driver.rs

@@ -76,10 +76,8 @@ where
             self.led1.set_high().ok();
             self.trace_path(&[
                 Point::new(0, 0),
-                Point::new(500, 0),
-                Point::new(500, 500),
                 Point::new(0, 500),
-                Point::new(0, 0),
+                Point::new_with_forward(0, 0, false),
             ]);
             self.led1.set_low().ok();
         }
@@ -109,19 +107,15 @@ where
             let distance = cur_point.distance_to(next_point);
             let bearing = cur_point.absolute_bearing(next_point);
             let bearing_diff = bearing - cur_bearing;
-
-            debug!(
-                "cur_point: {:?}, next_point: {:?}, distance: {}, bearing: {}, bearing_diff: {}",
-                cur_point, next_point, distance, bearing, bearing_diff
-            );
+            debug!("driving to next way point\n  cur_point: {:?}, next_point: {:?}\n  distance: {}, bearing {}, forward: {}", cur_point, next_point, distance, bearing, next_point.forward());
 
             // turn to the correct bearing
             if bearing_diff.abs() > self.robot.min_turn_angle() {
                 self.robot.turn(bearing_diff as i32);
             }
 
             // drive the distance
-            self.robot.straight(distance as u32);
+            self.robot.straight(distance as u32, next_point.forward());
 
             // update the current point and bearing
             cur_point = *next_point;

src/model/heading.rs

@@ -18,7 +18,7 @@ use mpu6050::Mpu6050;
 //      [-2241,-2240] --> [-1,15]	    [1803,1804] --> [-10,6]	    [1573,1574] --> [16371,16394]	[98,99] --> [-2,1]	    [44,45] --> [0,5]	    [19,20] --> [0,4]
 //      [-2239,-2238] --> [-3,16]	    [1804,1804] --> [0,5]	    [1575,1576] --> [16378,16395]	[98,99] --> [-1,2]	    [43,44] --> [-2,1]	    [19,20] --> [-1,2]
 
-const HEADING_UPDATE_INTERVAL_MS: u32 = 2;
+const HEADING_UPDATE_INTERVAL_MS: u32 = 1;
 
 pub struct HeadingCalculator<TWI, DELAY> {
     heading: f32,
@@ -104,10 +104,6 @@ where
                     } else if self.heading < -180.0 {
                         self.heading += 360.0;
                     }
-                    // debug!(
-                    //     "{}: delta_rads: {}, heading: {}",
-                    //     self.gyro, delta_rads, self.heading
-                    // );
                 }
                 Err(error) => {
                     error!("Error reading gyro: {}", error);

src/model/point.rs

@@ -6,11 +6,16 @@ use micromath::F32Ext;
 pub struct Point {
     x: i32,
     y: i32,
+    forward: bool,
 }
 
 impl Point {
     pub fn new(x: i32, y: i32) -> Self {
-        Self { x, y }
+        Point::new_with_forward(x, y, true)
+    }
+
+    pub fn new_with_forward(x: i32, y: i32, forward: bool) -> Self {
+        Self { x, y, forward }
     }
 
     pub fn x(&self) -> i32 {
@@ -21,6 +26,10 @@ impl Point {
         self.y
     }
 
+    pub fn forward(&self) -> bool {
+        self.forward
+    }
+
     /// The distance from this point to the other point
     pub fn distance_to(&self, other: &Point) -> f32 {
         let x_diff = self.x - other.x;
@@ -30,11 +39,24 @@ impl Point {
 
     /// The absolute bearing from this point to the other point, in degrees. 0 degrees pointing postively in the y axis, and
     /// the right hand rule is used to determine the angle. 90 degrees is pointing negatively in the x axis, 180 degrees
-    /// radians is pointing negatively in the y axis, and -90 degrees is pointing positively in the x axis
+    /// radians is pointing negatively in the y axis, and -90 degrees is pointing positively in the x axis.
+    /// Takes into account the forward direction of the robot.
     pub fn absolute_bearing(&self, other: &Point) -> f32 {
         let x_diff = -(other.x - self.x);
         let y_diff = other.y - self.y;
-        (x_diff as f32).atan2(y_diff as f32).to_degrees()
+        let bearing = (x_diff as f32).atan2(y_diff as f32).to_degrees();
+        if other.forward {
+            bearing
+        } else {
+            let reverse = bearing + 180.0;
+            if reverse > 180.0 {
+                reverse - 360.0
+            } else if reverse < -180.0 {
+                reverse + 360.0
+            } else {
+                reverse
+            }
+        }
     }
 }
 

src/robot.rs

@@ -294,11 +294,17 @@ where
         self
     }
 
-    pub fn straight(&mut self, distance_mm: u32) -> &mut Self {
+    pub fn straight(&mut self, distance_mm: u32, forward: bool) -> &mut Self {
+        let direction_arrow = if forward {
+            UP_ARROW_STRING
+        } else {
+            DOWN_ARROW_STRING
+        };
+
         if let Err(error) = core::write!(
             self.clear_lcd().set_lcd_cursor(0, 0),
             "{} 0 / {} mm",
-            UP_ARROW_STRING,
+            direction_arrow,
             distance_mm,
         ) {
             error!("Error writing to LCD: {}", error.to_string().as_str());
@@ -338,7 +344,11 @@ where
             0.,
         ));
         let mut update_count: u32 = 0;
-        self.motors.forward();
+        if forward {
+            self.motors.forward();
+        } else {
+            self.motors.reverse();
+        }
 
         while traveled_ticks < expected_ticks {
             cortex_m::interrupt::free(|cs| {
@@ -353,15 +363,15 @@ where
                 let heading = self.heading_calculator.heading();
                 let control_signal = controller.update(heading, last_update_millis);
 
-                let mut cs_indicator: &str = UP_ARROW_STRING;
+                let mut cs_indicator: &str = direction_arrow;
                 // positive control signal means turn left, a negative control signal means turn right
                 let mut left_power: f32 = 1.;
                 let mut right_power: f32 = 1.;
-                if control_signal > 0. {
+                if (forward && control_signal > 0.) || (!forward && control_signal < 0.) {
                     left_power = 1. - control_signal;
                     right_power = 1.0;
                     cs_indicator = LEFT_ARROW_STRING;
-                } else if control_signal < 0. {
+                } else if (forward && control_signal < 0.) || (!forward && control_signal > 0.) {
                     left_power = 1.0;
                     right_power = 1. + control_signal;
                     cs_indicator = RIGHT_ARROW_STRING;
@@ -385,7 +395,7 @@ where
                 if let Err(error) = core::write!(
                     self.set_lcd_cursor(0, 0),
                     "{} {} / {} mm",
-                    UP_ARROW_STRING,
+                    direction_arrow,
                     (traveled_ticks as f32 * MM_PER_WHEEL_TICK) as i32,
                     distance_mm,
                 ) {