117 lines
3.1 KiB
Rust
117 lines
3.1 KiB
Rust
#![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);
|
|
|
|
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");
|
|
|
|
// Variables for loop-persistent data
|
|
let mut switch_count = 0.0;
|
|
let mut endpoint = 75.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 => {
|
|
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;
|
|
}
|