326 lines
11 KiB
Rust
326 lines
11 KiB
Rust
use std::{fmt::format, ops::ControlFlow, result, sync::{atomic::Ordering, Arc}, thread::{self, sleep}, time::Duration};
|
|
|
|
use anyhow::{Context, Ok, Result};
|
|
use atomic_float::AtomicF32;
|
|
use interface::{auto::get_confs, combatlog::{combat_logger, CombatData}, POWER_THRESHOLD};
|
|
use common::{ControlPacket, TelemetryPacket};
|
|
use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
|
|
use egui_toast::{Toast, ToastKind};
|
|
use interface::gui::{gui, GUIData, DEFAULT_VOLUME_THRESHOLD, GUI};
|
|
use pitch_detection::{detector::{mcleod::McLeodDetector, PitchDetector}, utils};
|
|
use rust_music_theory::note::{Note, NoteLetter, Pitch, Tuning};
|
|
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, broadcast, mpsc, watch, RwLock}, time::timeout};
|
|
|
|
|
|
fn main() -> Result<()> {
|
|
get_confs().unwrap();
|
|
panic!("got too far");
|
|
|
|
let (logging_sender, combatlog) = mpsc::channel(64);
|
|
|
|
// assumes pulseaudio system with f32 samples and 2204 sample packets
|
|
dbg!(cpal::available_hosts());
|
|
let host = cpal::default_host();
|
|
#[cfg(target_os = "windows")]
|
|
let Some(device) = host.devices().unwrap().filter(|device| device.supported_input_configs().unwrap().count() != 0).next() else {
|
|
panic!("no input devices")
|
|
};
|
|
#[cfg(target_os = "linux")]
|
|
let device = host.devices().unwrap().find(|d|d.name().unwrap() == "pulse").context("no pulse")?;
|
|
|
|
let config = device.default_input_config()?;
|
|
|
|
dbg!(config.sample_format());
|
|
let rate = config.sample_rate();
|
|
|
|
let (sender, notes) = broadcast::channel(2);
|
|
|
|
const CLARITY_THRESHOLD: f32 = 0.85;
|
|
#[cfg(target_os = "linux")]
|
|
const PACKET_LEN: usize = 2204;
|
|
#[cfg(target_os = "windows")]
|
|
const PACKET_LEN: usize = 1800;
|
|
|
|
let mut packet = Vec::new();
|
|
|
|
let stream = device.build_input_stream(&config.into(),
|
|
move | data: &[f32], _: &_| {
|
|
let mut data = data;
|
|
|
|
if packet.len() > PACKET_LEN {
|
|
packet.clear();
|
|
}
|
|
|
|
if data.len() <= PACKET_LEN {
|
|
packet.extend_from_slice(data);
|
|
if packet.len() > PACKET_LEN {
|
|
data = &packet;
|
|
} else {
|
|
return;
|
|
}
|
|
}
|
|
assert!(data.len() >= PACKET_LEN);
|
|
let data = &data[..PACKET_LEN];
|
|
|
|
// reinitialized every packet as it is not thread safe
|
|
let mut detctor = McLeodDetector::new(PACKET_LEN, PACKET_LEN/2);
|
|
let vol = utils::buffer::square_sum(data);
|
|
if let Err(e) = sender.send((detctor.get_pitch(data, rate.0 as usize, POWER_THRESHOLD.load(Ordering::Relaxed), CLARITY_THRESHOLD).map(|p|p.frequency),vol)) {
|
|
println!("channel: {e}");
|
|
}
|
|
|
|
},
|
|
move |err| {eprintln!("{err}")} ,
|
|
Some(Duration::from_secs(1)))?;
|
|
|
|
stream.play()?;
|
|
|
|
let executor = tokio::runtime::Runtime::new()?;
|
|
|
|
//let latest_telem = Arc::new(RwLock::new(None));
|
|
|
|
let (toast_sender, toasts) = mpsc::channel(5);
|
|
let (data_sender, gui_data) =watch::channel(GUIData::default());
|
|
|
|
|
|
let spawner = executor.handle().clone();
|
|
thread::spawn(move || {
|
|
spawner.block_on(async {
|
|
let log_toasts = toast_sender.clone();
|
|
tokio::spawn(async move {
|
|
if let Err(e) = combat_logger(combatlog).await {
|
|
let _ = log_toasts.clone().send(Toast::new().text(format!("logger crashed: {e:?}")).kind(ToastKind::Error)).await;
|
|
}
|
|
});
|
|
|
|
loop {
|
|
if let Err(e) = connect(toast_sender.clone(), notes.resubscribe(), data_sender.clone(), logging_sender.clone()).await {
|
|
if let Err(_) = toast_sender.send(Toast::new().text(format!("{e:?}")).kind(ToastKind::Error)).await {
|
|
break;
|
|
};
|
|
}
|
|
}
|
|
});
|
|
|
|
});
|
|
|
|
println!("launching gui");
|
|
gui(gui_data, toasts, executor).unwrap();
|
|
|
|
drop(stream);
|
|
|
|
Ok(())
|
|
}
|
|
|
|
/// frequency, volume
|
|
type Detection = (Option<f32>, f32);
|
|
|
|
async fn connect(toast_sender: mpsc::Sender<Toast>, notes: broadcast::Receiver<Detection>, data_sender: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>) -> Result<()>{
|
|
toast_sender.send(Toast::new().text("connecting to bot").kind(ToastKind::Info)).await?;
|
|
let cruisecontrol = TcpStream::connect("192.168.1.2:1234").await?;
|
|
println!("connected");
|
|
toast_sender.send(Toast::new().text("connected").kind(ToastKind::Success)).await?;
|
|
cruisecontrol.set_nodelay(true)?;
|
|
let (telem, control) = cruisecontrol.into_split();
|
|
|
|
tokio::spawn(telemetry_handler(telem, data_sender.clone(), logging_sender.clone()));
|
|
|
|
controller(notes, control, data_sender.clone(), logging_sender.clone()).await?;
|
|
Ok(())
|
|
}
|
|
|
|
async fn telemetry_handler(mut telem: OwnedReadHalf, gui: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>) -> Result<()> {
|
|
let mut buf = vec![0; 2048];
|
|
loop {
|
|
let len = telem.read_u32().await.context("bad length")? as usize;
|
|
let data = &mut buf[0..len];
|
|
telem.read_exact(data).await?;
|
|
let telem: TelemetryPacket = postcard::from_bytes(&data)?;
|
|
|
|
//println!("telem: {telem:?}");
|
|
|
|
logging_sender.send(CombatData::Telemetry(telem.clone())).await?;
|
|
|
|
gui.send_modify(|gui| {
|
|
gui.telemetry = Some(telem);
|
|
});
|
|
GUI.get().map(|c| c.request_repaint());
|
|
}
|
|
}
|
|
|
|
async fn controller(mut notes: broadcast::Receiver<Detection>, controller: OwnedWriteHalf, gui: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>) -> Result<()> {
|
|
let mut controller = BufWriter::new(controller);
|
|
//send_packet(&mut controller, ControlPacket::Arm(true)).await?;
|
|
//println!("armed flipper");
|
|
|
|
//let (auto_tx, mut auto_rx) = mpsc::channel(1);
|
|
//let mut auto = None;
|
|
|
|
//auto = Some(spawn(seek))
|
|
|
|
loop {
|
|
let mut control = ControlPacket::Stop;
|
|
|
|
let result::Result::Ok(note) = timeout(Duration::from_millis(250), notes.recv()).await else {
|
|
println!("timeout");
|
|
send_packet(&mut controller, ControlPacket::Stop).await?;
|
|
continue;
|
|
};
|
|
|
|
let (note,vol) = note.context("channel closed")?;
|
|
if let Some(note) = note {
|
|
dbg!(note);
|
|
if let ControlFlow::Break(_) = sax_control(&mut control, vol, note) {
|
|
if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
send_packet(&mut controller, control.clone()).await?;
|
|
logging_sender.send(CombatData::Control(control.clone())).await?;
|
|
gui.send_modify(|gui| gui.last_command = Some(control));
|
|
} else {
|
|
send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
|
|
gui.send_modify(|gui| gui.last_command = Some(ControlPacket::Stop));
|
|
}
|
|
GUI.get().map(|c| c.request_repaint());
|
|
}
|
|
}
|
|
|
|
/// Weapon enabled
|
|
const ARMED: bool = true;
|
|
|
|
fn sax_control(control: &mut ControlPacket, vol: f32, frequency: f32) -> ControlFlow<()> {
|
|
if frequency < 150. {
|
|
println!("too low");
|
|
return ControlFlow::Break(());
|
|
}
|
|
if frequency > 270. {
|
|
println!("too high");
|
|
return ControlFlow::Break(());
|
|
}
|
|
let note = Note::from_freq(frequency, Tuning::EqualTemperament);
|
|
//dbg!(note.clarity);
|
|
//dbg!(vol);
|
|
match note.pitch {
|
|
Pitch { letter: NoteLetter::C, accidental: 1} => {
|
|
println!("forward");
|
|
*control = ControlPacket::Twist(1.0, 0.0);
|
|
}
|
|
//Pitch { letter: NoteLetter::A, accidental: 1} => {
|
|
// println!("backward");
|
|
// *control = ControlPacket::Twist(-1.0, 0.0);
|
|
//}
|
|
Pitch { letter: NoteLetter::B, accidental: 0} => {
|
|
println!("right");
|
|
*control = ControlPacket::Twist(0.0, 1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::A, accidental: 0} => {
|
|
println!("left");
|
|
*control = ControlPacket::Twist(0.0, -1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::G, accidental: 1} => {
|
|
if ARMED {
|
|
println!("fire");
|
|
*control = ControlPacket::Fire;
|
|
}
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 1} => {
|
|
println!("stop flat");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
pitch => {
|
|
if vol > 3000. {
|
|
println!("rly loud");
|
|
//control = ControlPacket::FireOverride(1.0);
|
|
dbg!(pitch);
|
|
} else {
|
|
dbg!(pitch);
|
|
}
|
|
}
|
|
}
|
|
ControlFlow::Continue(())
|
|
}
|
|
|
|
|
|
fn recorder_control(control: &mut ControlPacket, vol: f32, frequency: f32) -> ControlFlow<()> {
|
|
if frequency < 300. {
|
|
println!("too low");
|
|
return ControlFlow::Break(());
|
|
}
|
|
if frequency > 600. {
|
|
println!("too high");
|
|
return ControlFlow::Break(());
|
|
}
|
|
let note = Note::from_freq(frequency, Tuning::EqualTemperament);
|
|
//dbg!(note.clarity);
|
|
//dbg!(vol);
|
|
match note.pitch {
|
|
Pitch { letter: NoteLetter::A, accidental: 0} => {
|
|
println!("forward");
|
|
*control = ControlPacket::Twist(1.0, 0.0);
|
|
}
|
|
Pitch { letter: NoteLetter::A, accidental: 1} => {
|
|
println!("backward");
|
|
*control = ControlPacket::Twist(-1.0, 0.0);
|
|
}
|
|
Pitch { letter: NoteLetter::C, accidental: 0} => {
|
|
println!("right");
|
|
*control = ControlPacket::Twist(0.0, 1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::C, accidental: 1} => {
|
|
println!("left");
|
|
*control = ControlPacket::Twist(0.0, -1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::G, accidental: 0|1} => {
|
|
if ARMED {
|
|
println!("fire");
|
|
*control = ControlPacket::Fire;
|
|
}
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 1} => {
|
|
println!("stop flat");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 0} => {
|
|
println!("stop");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
Pitch { letter: NoteLetter::E, accidental: 0} => {
|
|
println!("stop");
|
|
*control = ControlPacket::Stop;
|
|
//if let result::Result::Ok(command) = auto_rx.try_recv() {
|
|
// control = command;
|
|
//}
|
|
}
|
|
pitch => {
|
|
if vol > 3000. {
|
|
println!("rly loud");
|
|
//control = ControlPacket::FireOverride(1.0);
|
|
} else {
|
|
dbg!(pitch);
|
|
}
|
|
}
|
|
}
|
|
ControlFlow::Continue(())
|
|
}
|
|
|
|
async fn send_packet(controller: &mut BufWriter<OwnedWriteHalf>, control: ControlPacket) -> Result<(), anyhow::Error> {
|
|
let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
|
|
controller.write_u32(control.len() as u32).await?;
|
|
controller.write_all(&control).await?;
|
|
controller.flush().await?;
|
|
Ok(())
|
|
}
|
|
|
|
//async fn seek(data: TelemetryPacket) -> Result<()>{
|
|
//
|
|
// let left_tof = data.sensors.tof1.context("missing tof1")?;
|
|
// let right_tof = data.sensors.tof2.context("missing tof2")?;
|
|
//
|
|
// /// Distance (mm) where both tofs point at the same thing
|
|
// const CONVERGANCE_DISTANCE: u16 = 80;
|
|
// const DETECT_DISTANCE: u16 = 380;
|
|
//
|
|
//
|
|
//}
|