123 lines
3.7 KiB
Rust
123 lines
3.7 KiB
Rust
use std::time::Duration;
|
|
|
|
use anyhow::Context;
|
|
use common::{Command, SensorData};
|
|
use gilrs::{Axis, Event, Gilrs};
|
|
use nalgebra::Vector3;
|
|
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}};
|
|
|
|
#[tokio::main]
|
|
async fn main() -> anyhow::Result<()> {
|
|
println!("Hello, world!");
|
|
|
|
let mut gamepads = Gilrs::new().unwrap();
|
|
let (_, gamepad) = gamepads.gamepads().nth(0).context("no gamepads")?;
|
|
let ly = gamepad.axis_code(Axis::LeftStickY).context("no left joystick")?;
|
|
let rx = gamepad.axis_code(Axis::RightStickX).context("no right joystick")?;
|
|
let lb = gamepad.button_code(gilrs::Button::LeftTrigger).context("no lb")?;
|
|
let a = gamepad.button_code(gilrs::Button::South).context("no a")?;
|
|
let mut last_a = false;
|
|
|
|
let server = "130.215.211.79:3322";
|
|
let mut robot = TcpStream::connect(server).await?;
|
|
robot.set_nodelay(true)?;
|
|
|
|
let (telem, robot_controller) = robot.into_split();
|
|
|
|
tokio::spawn(print_telem(telem));
|
|
|
|
let mut robot_controller = BufWriter::new(robot_controller);
|
|
|
|
let mut active_gamepad = None;
|
|
let mut last_event = Instant::now();
|
|
|
|
loop {
|
|
while let Some(Event { id,..}) = gamepads.next_event() {
|
|
active_gamepad = Some(id);
|
|
last_event = Instant::now();
|
|
}
|
|
|
|
let Some(gamepad) = active_gamepad else {
|
|
continue;
|
|
};
|
|
|
|
let gamepad = gamepads.gamepad(gamepad);
|
|
|
|
let values = gamepad.state();
|
|
|
|
let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
|
|
let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
|
|
|
|
let boost = values.button_data(lb).map(|b| b.is_pressed()).unwrap_or(false);
|
|
|
|
let a = values.button_data(a).map(|b| b.is_pressed()).unwrap_or(false);
|
|
|
|
for axis in [&mut rx, &mut ly] {
|
|
if axis.abs() < 0.05 {
|
|
*axis = 0.0;
|
|
}
|
|
|
|
if !boost {
|
|
*axis *= 0.3;
|
|
}
|
|
}
|
|
|
|
if !gamepad.is_connected() {
|
|
ly = 0.0;
|
|
rx = 0.0;
|
|
}
|
|
|
|
let cmd = Command::Twist(ly, rx);
|
|
println!("twist");
|
|
|
|
let encoded = postcard::to_stdvec(&cmd)?;
|
|
robot_controller.write_u32(encoded.len() as u32).await?;
|
|
robot_controller.write_all(&encoded).await?;
|
|
|
|
if a != last_a {
|
|
let black = Vector3::repeat(0u8);
|
|
let green = Vector3::new(254u8, 0, 254);
|
|
let cmd = Command::SetLed(if a {green} else {black});
|
|
let encoded = postcard::to_stdvec(&cmd)?;
|
|
robot_controller.write_u32(encoded.len() as u32).await?;
|
|
robot_controller.write_all(&encoded).await?;
|
|
println!("led");
|
|
}
|
|
last_a = a;
|
|
|
|
|
|
if let Err(e) = robot_controller.flush().await {
|
|
println!("flush fail: {e}, reconnecting");
|
|
robot = TcpStream::connect(server).await?;
|
|
robot.set_nodelay(true)?;
|
|
|
|
let (telem, robot) = robot.into_split();
|
|
|
|
tokio::spawn(print_telem(telem));
|
|
|
|
robot_controller = BufWriter::new(robot);
|
|
};
|
|
|
|
sleep(Duration::from_millis(65)).await;
|
|
}
|
|
}
|
|
|
|
async fn print_telem(mut telem: OwnedReadHalf) -> anyhow::Result<()> {
|
|
loop {
|
|
let len = telem.read_u32().await? as usize;
|
|
let mut buf = vec![0; len];
|
|
telem.read_exact(&mut buf).await?;
|
|
let telem: SensorData = postcard::from_bytes(&buf)?;
|
|
|
|
match telem {
|
|
SensorData::BusVoltage(voltage) => {
|
|
println!("voltage: {voltage}");
|
|
}
|
|
SensorData::AmbientTemperature(temp) => {
|
|
println!("pico temp {}", temp * 9./5. + 32.);
|
|
}
|
|
_ => {}
|
|
}
|
|
|
|
}
|
|
}
|