From 55794a6188ddf379be3cd8e49c12bc85d3db9bb8 Mon Sep 17 00:00:00 2001
From: Erin Abicht <contact@the-system.eu.org>
Date: Sun, 2 Jul 2023 00:47:13 -0400
Subject: [PATCH] Switch w/ smoothing

---
 Cargo.lock  |  1 +
 Cargo.toml  |  7 ++++++
 src/main.rs | 63 +++++++++++++++++++++++++++++++++++++++++++----------
 3 files changed, 60 insertions(+), 11 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 5c85158..5f58bd0 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -87,6 +87,7 @@ name = "cybertail"
 version = "0.1.0"
 dependencies = [
  "arduino-hal",
+ "avr-device",
  "embedded-hal",
  "nb 0.1.3",
  "panic-halt",
diff --git a/Cargo.toml b/Cargo.toml
index a504909..a7391b1 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -21,6 +21,9 @@ git = "https://github.com/rahix/avr-hal"
 rev = "7dfa6d322b9df98b2d98afe0e14a97afe0187ac1"
 features = ["arduino-mega2560"]
 
+[dependencies.avr-device]
+version = "0.5.1"
+
 # Configure the build for minimal size - AVRs have very little program memory
 [profile.dev]
 panic = "abort"
@@ -33,3 +36,7 @@ codegen-units = 1
 debug = true
 lto = true
 opt-level = "s"
+
+# Dirty Hack: https://github.com/Rahix/avr-hal/issues/131#issuecomment-775828622
+[profile.dev.package.compiler_builtins]
+overflow-checks = false
diff --git a/src/main.rs b/src/main.rs
index bc6eb10..cfd6502 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,6 +3,42 @@
 
 use arduino_hal::port::{Pin, mode::Output};
 use panic_halt as _;
+use avr_device::interrupt;
+use core::cell::RefCell;
+
+type Console = arduino_hal::hal::usart::Usart0<arduino_hal::DefaultClock>;
+static CONSOLE: interrupt::Mutex<RefCell<Option<Console>>> =
+    interrupt::Mutex::new(RefCell::new(None));
+
+macro_rules! print {
+    ($($t:tt)*) => {
+        interrupt::free(
+            |cs| {
+                if let Some(console) = CONSOLE.borrow(cs).borrow_mut().as_mut() {
+                    let _ = ufmt::uwrite!(console, $($t)*);
+                }
+            },
+        )
+    };
+}
+
+macro_rules! println {
+    ($($t:tt)*) => {
+        interrupt::free(
+            |cs| {
+                if let Some(console) = CONSOLE.borrow(cs).borrow_mut().as_mut() {
+                    let _ = ufmt::uwriteln!(console, $($t)*);
+                }
+            },
+        )
+    };
+}
+
+fn put_console(console: Console) {
+    interrupt::free(|cs| {
+        *CONSOLE.borrow(cs).borrow_mut() = Some(console);
+    })
+}
 
 const ANGLE_PULSE_RATIO: f32 = 10.25;
 
@@ -18,20 +54,25 @@ fn set_servo_degree(pin: &mut Pin<Output>, degree: f32) {
 fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
-
-    /*
-     * For examples (and inspiration), head to
-     *
-     *     https://github.com/Rahix/avr-hal/tree/main/examples
-     *
-     * NOTE: Not all examples were ported to all boards!  There is a good chance though, that code
-     * for a different board can be adapted for yours.  The Arduino Uno currently has the most
-     * examples available.
-     */
+    let serial = arduino_hal::default_serial!(dp, pins, 57600);
+     put_console(serial);
 
     let mut servo = pins.d9.into_output().downgrade();
+    let switch = pins.d10.into_pull_up_input();
+    println!("hewwo :3");
+
+    let mut switchSmoothed: f32 = 0.0;
+    let mut switchPrev: f32 = 0.0;
 
     loop {
-        set_servo_degree(&mut servo, 180.0);
+        let val = match switch.is_low() {
+            true => 1.0,
+            false => 0.0,
+        };
+        switchSmoothed = (val * 0.05) + (switchPrev * 0.95);
+        switchPrev = switchSmoothed;
+        let deg = (190.0 * switchSmoothed) + 25.0;
+
+        set_servo_degree(&mut servo, deg);
     }
 }