Second switch, directional control, set endpoint, lerp

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",

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

src/main.rs

@@ -1,27 +1,116 @@
 #![no_std]
 #![no_main]
 
+use arduino_hal::port::{Pin, mode::Output};
 use panic_halt as _;
+use avr_device::interrupt;
+use core::cell::RefCell;
+
+// Serial monitor shit
+// No idea wtf this does
+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);
+    })
+}
+
+// Test ratio for converting from angle to servo control pulse
+// This was a guess and is kinda innacurate
+// Should do more accurate testing for final version
+const ANGLE_PULSE_RATIO: f32 = 10.25;
+
+fn set_servo_degree(pin: &mut Pin<Output>, degree: f32) {
+    // Using delays blocks the processor
+    // Cannot work with multiple motors
+    // Embassy async will fix this on pico
+    let delay = (degree * ANGLE_PULSE_RATIO) as u32;
+    pin.set_high();
+    arduino_hal::delay_us(delay);
+    pin.set_low();
+    arduino_hal::delay_us(20000-delay);
+}
 
 #[arduino_hal::entry]
 fn main() -> ! {
+    /* Setup Pins */
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
+    let serial = arduino_hal::default_serial!(dp, pins, 57600);
+    put_console(serial);
 
-    /*
-     * 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 mut servo = pins.d9.into_output().downgrade();
+    let switch = pins.d10.into_pull_up_input();
+    let switch2 = pins.d11.into_pull_up_input();
+    println!("hewwo :3");
 
-    let mut led = pins.d13.into_output();
+    // Variables for loop-persistent data
+    let mut switch_count = 0.0;
+    let mut endpoint = 75.0;
 
     loop {
-        led.toggle();
-        arduino_hal::delay_ms(1000);
+        /* Get Input */
+        // This debouncing sucks ass and isn't good - doesn't matter, just for testing
+        switch_count = match switch.is_low() {
+            true => {
+                endpoint = 170.0;
+                1.0
+            },
+            false => {
+                if switch_count > 0.0 {
+                    switch_count - 0.1
+                } else {
+                    0.0
+                }
+            },
+        };
+        switch_count = match switch2.is_low() {
+            true => {
+                endpoint = 5.0;
+                1.0
+            },
+            false => {
+                if switch_count > 0.0 {
+                    switch_count - 0.1
+                } else {
+                    0.0
+                }
+            },
+        };
+
+        /* Move Servos */
+        let deg = lerp(75.0, endpoint, switch_count);
+        set_servo_degree(&mut servo, deg);
     }
 }
+
+fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
+    return (1.0 - t) * v0 + t * v1;
+}