diff --git a/node-hub/dora-rustypot/Cargo.toml b/node-hub/dora-rustypot/Cargo.toml new file mode 100644 index 00000000..0b619bed --- /dev/null +++ b/node-hub/dora-rustypot/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "dora-rustypot" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +dora-node-api = "0.3.11" +eyre = "0.6.12" +rustypot = { git = "https://github.com/pollen-robotics/rustypot", branch = "next-release-1.0" } +serialport = "4.7.1" diff --git a/node-hub/dora-rustypot/src/main.rs b/node-hub/dora-rustypot/src/main.rs new file mode 100644 index 00000000..dfdfc2e6 --- /dev/null +++ b/node-hub/dora-rustypot/src/main.rs @@ -0,0 +1,76 @@ +use dora_node_api::dora_core::config::DataId; +use dora_node_api::{into_vec, DoraNode, Event, IntoArrow, Parameter}; +use eyre::{Context, Result}; +use rustypot::servo::feetech::sts3215::Sts3215Controller; +use std::collections::BTreeMap; +use std::time::Duration; + +fn main() -> Result<()> { + let (mut node, mut events) = DoraNode::init_from_env()?; + let serialportname: String = std::env::var("PORT").context("Serial port name not provided")?; + let baudrate: u32 = std::env::var("BAUDRATE") + .unwrap_or_else(|_| "1000000".to_string()) + .parse() + .context("Invalid baudrate")?; + let ids = std::env::var("IDS") + .unwrap_or_else(|_| "1,2,3,4,5,6".to_string()) + .split(&[',', ' '][..]) + .map(|s| s.parse::().unwrap()) + .collect::>(); + let torque = std::env::var("TORQUE") + .unwrap_or_else(|_| "false".to_string()) + .parse::() + .context("Invalid torque")?; + + let serial_port = serialport::new(serialportname, baudrate) + .timeout(Duration::from_millis(1000)) + .open()?; + + let mut c = Sts3215Controller::new() + .with_protocol_v1() + .with_serial_port(serial_port); + + if torque { + let truthies = vec![true; ids.len()]; + c.write_torque_enable(&ids, &truthies) + .expect("could not enable torque"); + } else { + let falsies = vec![false; ids.len()]; + c.write_torque_enable(&ids, &falsies) + .expect("could not enable torque"); + } + + while let Some(event) = events.recv() { + match event { + Event::Input { + id, + metadata: _, + data, + } => match id.as_str() { + "tick" => { + if let Ok(joints) = c.read_present_position(&ids) { + let mut parameter = BTreeMap::new(); + parameter.insert( + "encoding".to_string(), + Parameter::String("jointstate".to_string()), + ); + node.send_output( + DataId::from("pose".to_string()), + parameter, + joints.into_arrow(), + ) + .unwrap(); + }; + } + "pose" => { + let data: Vec = into_vec(&data).expect("could not cast values"); + c.write_goal_position(&ids, &data).unwrap(); + } + other => eprintln!("Received input `{other}`"), + }, + _ => {} + } + } + + Ok(()) +}