From 25930e6b6e4fd20fb7ae01665e5352433da4c776 Mon Sep 17 00:00:00 2001
From: Michael Kamprath <michael@kamprath.net>
Date: Wed, 8 May 2024 12:12:34 -0700
Subject: [PATCH] added gyro status display

---
 src/driver.rs |  9 +++++++--
 src/robot.rs  | 30 ++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/src/driver.rs b/src/driver.rs
index bd0456c..2dad230 100644
--- a/src/driver.rs
+++ b/src/driver.rs
@@ -252,7 +252,7 @@ where
         // - button one will select the current function and execute it
         // - if no button is pressed for the idle wait time, return to idle state
 
-        const FUNCTIONS: [&str; 2] = ["Select Path", "Calibrate Gyro"];
+        const FUNCTIONS: [&str; 3] = ["Select Path", "Calibrate Gyro", "Heading Display"];
         let mut function_idx: usize = 0;
         let mut last_interaction_millis = millis();
         self.robot.clear_display_reset_timer();
@@ -290,6 +290,7 @@ where
                             self.robot
                                 .set_idle_message_line2(self.selected_path.clone());
                         }
+                        self.robot.start_display_reset_timer();
                     }
                     1 => {
                         write!(
@@ -299,12 +300,16 @@ where
                         .ok();
                         self.robot.calibrate_gyro(&mut self.delay);
                         write!(self.robot.set_lcd_cursor(0, 1), "      Done      ",).ok();
+                        self.robot.start_display_reset_timer();
+                    }
+                    2 => {
+                        self.robot.display_heading().ok();
+                        self.robot.set_display_to_idle();
                     }
                     _ => {
                         warn!("handle_functions_menu: invalid function index");
                     }
                 }
-                self.robot.start_display_reset_timer();
                 return;
             }
         }
diff --git a/src/robot.rs b/src/robot.rs
index db8adab..11ffa9a 100644
--- a/src/robot.rs
+++ b/src/robot.rs
@@ -684,6 +684,36 @@ where
         self
     }
 
+    //--------------------------------------------------------------------------
+    // Test functions
+    //--------------------------------------------------------------------------
+    pub fn display_heading(&mut self) -> Result<(), adafruit_lcd_backpack::Error<TWI_ERR>> {
+        write!(self.lcd.clear()?.set_cursor(0, 0)?, "Heading:").ok();
+        self.heading_calculator.reset();
+        let mut continue_loop = true;
+
+        let mut last_update_millis = 0;
+        while continue_loop {
+            self.handle_loop();
+            if self.button1_pressed() || self.button2_pressed() {
+                continue_loop = false;
+            }
+            if millis() - last_update_millis > 500 {
+                let heading = self.heading_calculator.heading();
+                if let Err(error) = core::write!(
+                    self.lcd.set_cursor(0, 1)?,
+                    "{:.2}{: <16}",
+                    heading,
+                    DEGREES_STRING
+                ) {
+                    error!("Error writing to LCD: {}", error.to_string().as_str());
+                }
+                last_update_millis = millis();
+            }
+        }
+
+        Ok(())
+    }
     //--------------------------------------------------------------------------
     // LCD functions
     //--------------------------------------------------------------------------