diff --git a/src/main.rs b/src/main.rs index bae4541..dae6de9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -6,6 +6,8 @@ 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; static CONSOLE: interrupt::Mutex>> = interrupt::Mutex::new(RefCell::new(None)); @@ -40,9 +42,15 @@ fn put_console(console: 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, 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); @@ -52,21 +60,24 @@ fn set_servo_degree(pin: &mut Pin, degree: f32) { #[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); + put_console(serial); let mut servo = pins.d9.into_output().downgrade(); let switch = pins.d10.into_pull_up_input(); println!("hewwo :3"); + // Variables for loop-persistent data let mut switchSmoothed: f32 = 0.0; let mut switchPrev: f32 = 0.0; let mut switch_count = 0.0; loop { /* Get Input */ + // This debouncing sucks ass and isn't good - doesn't matter, just for testing switch_count = match switch.is_low() { true => 1.0, false => switch_count - 0.2, @@ -79,6 +90,7 @@ fn main() -> ! { val = 0.0; } + /* Move Servos */ switchSmoothed = (switch_count * 0.05) + (switchPrev * 0.95); switchPrev = switchSmoothed; let deg = (190.0 * switchSmoothed) + 25.0;