103 lines
3.4 KiB
Rust
103 lines
3.4 KiB
Rust
use std::{io::{BufRead, BufReader, BufWriter, Read, Write}, net::TcpListener, sync::{Arc, Mutex}, thread::sleep, time::Duration};
|
|
|
|
use common::{Command, Telemetry};
|
|
use postcard::to_vec_cobs;
|
|
use heapless::Vec;
|
|
|
|
fn main() {
|
|
let mut port = serialport::new("/dev/ttyACM0", 115200)
|
|
.timeout(Duration::from_secs(1000))
|
|
.open().expect("port missing");
|
|
println!("connected over serial");
|
|
|
|
let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: true}));
|
|
let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));
|
|
|
|
// LabVIEW communication thread
|
|
{
|
|
let command = command.clone();
|
|
let telemetry = telemetry.clone();
|
|
std::thread::spawn(move || server(command,telemetry));
|
|
}
|
|
|
|
// Telemetry receive thread
|
|
{
|
|
let reader = port.try_clone().unwrap();
|
|
let reader = BufReader::new(reader);
|
|
let telemetry = telemetry.clone();
|
|
std::thread::spawn(move || recv_telem(telemetry, reader));
|
|
}
|
|
|
|
// Command transmit thread (main)
|
|
loop {
|
|
let command = command.lock().unwrap().clone();
|
|
let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
|
|
if port.write(&encoded).is_err() {
|
|
println!("disconnected from robot");
|
|
return;
|
|
};
|
|
|
|
sleep(Duration::from_millis(200));
|
|
}
|
|
}
|
|
|
|
fn recv_telem<T: Read>(telemetry: Arc<Mutex<Telemetry>>, mut reader: BufReader<T>) {
|
|
let mut recv_buf = std::vec::Vec::new();
|
|
loop {
|
|
let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
|
|
if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
|
|
*telemetry.lock().unwrap() = telem;
|
|
}
|
|
recv_buf.clear();
|
|
}
|
|
}
|
|
|
|
fn server(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>) {
|
|
let listener = TcpListener::bind("0.0.0.0:4242").unwrap();
|
|
|
|
while let Ok((stream, _)) = listener.accept() {
|
|
println!("connected over IP");
|
|
let command1 = command.clone();
|
|
let telemetry1 = telemetry.clone();
|
|
std::thread::spawn(move || {
|
|
if let Err(e) = handle_connection(command1, telemetry1, stream) {
|
|
println!("IP connection dropped: {}", e);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
fn handle_connection(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>, stream: std::net::TcpStream) -> anyhow::Result<()> {
|
|
let writer = stream.try_clone().unwrap();
|
|
let mut writer = BufWriter::new(writer);
|
|
let mut reader = BufReader::new(stream);
|
|
|
|
loop {
|
|
let mut data_buf = std::vec::Vec::new();
|
|
let len = reader.read_until(b'\n', &mut data_buf)?;
|
|
|
|
// eof
|
|
if len == 0 {
|
|
*command.lock().unwrap() = Command::default(); // stop motors
|
|
println!("disconnected from LabVIEW");
|
|
return anyhow::Ok(());
|
|
}
|
|
|
|
|
|
let Ok(new_command) = serde_json::from_slice::<Command>(&data_buf[0..len]) else {continue;};
|
|
|
|
*command.lock().unwrap() = new_command.clone();
|
|
println!("received l{} r{}", new_command.left, new_command.right);
|
|
|
|
let mut telem = telemetry.lock().unwrap().clone();
|
|
|
|
if telem.distance.is_nan() {
|
|
telem.distance = -1.0;
|
|
}
|
|
println!("sent d{}", telem.distance);
|
|
|
|
serde_json::to_writer(&mut writer, &telem).unwrap();
|
|
writer.write_all(b"\r\n")?;
|
|
writer.flush()?;
|
|
}
|
|
}
|